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

Merged in feature/BAD_custom_chart (pull request #47) 

A failing unit test for a custom chart
release/4.3a0
Frank Dellaert 2014-11-26 11:50:14 +01:00
parent e842c9adbc c2e38633b5
commit 846513cdb5
9 changed files with 566 additions and 394 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -2777,6 +2777,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testCustomChartExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

66
gtsam/base/numericalDerivative.h
View File

@ -524,71 +524,5 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
delta);
}
// The benefit of this method is that it does not need to know what types are involved
// to evaluate the factor. If all the machinery of gtsam is working correctly, we should
// get the correct numerical derivatives out the other side.
template<typename FactorType>
JacobianFactor computeNumericalDerivativeJacobianFactor(const FactorType& factor,
const Values& values,
double fd_step) {
Eigen::VectorXd e = factor.unwhitenedError(values);
const size_t rows = e.size();
std::map<Key, Matrix> jacobians;
typename FactorType::const_iterator key_it = factor.begin();
VectorValues dX = values.zeroVectors();
for(; key_it != factor.end(); ++key_it) {
size_t key = *key_it;
// Compute central differences using the values struct.
const size_t cols = dX.dim(key);
Matrix J = Matrix::Zero(rows, cols);
for(size_t col = 0; col < cols; ++col) {
Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
dx[col] = fd_step;
dX[key] = dx;
Values eval_values = values.retract(dX);
Eigen::VectorXd left = factor.unwhitenedError(eval_values);
dx[col] = -fd_step;
dX[key] = dx;
eval_values = values.retract(dX);
Eigen::VectorXd right = factor.unwhitenedError(eval_values);
J.col(col) = (left - right) * (1.0/(2.0 * fd_step));
}
jacobians[key] = J;
}
// Next step...return JacobianFactor
return JacobianFactor(jacobians, -e);
}
template<typename FactorType>
void testFactorJacobians(TestResult& result_,
const std::string& name_,
const FactorType& f,
const gtsam::Values& values,
double fd_step,
double tolerance) {
// Check linearization
JacobianFactor expected = computeNumericalDerivativeJacobianFactor(f, values, fd_step);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
Jacobian evalJ = jf->jacobianUnweighted();
Jacobian estJ = expected.jacobianUnweighted();
EXPECT(assert_equal(evalJ.first, estJ.first, tolerance));
EXPECT(assert_equal(evalJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance));
EXPECT(assert_equal(estJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance));
}
} // namespace gtsam
/// \brief Check the Jacobians produced by a factor against finite differences.
/// \param factor The factor to test.
/// \param values Values filled in for testing the Jacobians.
/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
/// \param tolerance The numerical tolerance to use when comparing Jacobians.
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
{ gtsam::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); }

489
gtsam/nonlinear/NonlinearEquality.h
View File

@ -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

8
gtsam/nonlinear/Values-inl.h
View File

@ -302,8 +302,8 @@ namespace gtsam {
}
// overloaded insert with chart initializer
template<typename ValueType, typename Chart, typename ChartInit>
void Values::insert(Key j, const ValueType& val, ChartInit chart) {
template<typename ValueType, typename Chart>
void Values::insert(Key j, const ValueType& val, Chart chart) {
insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
}
@ -320,8 +320,8 @@ namespace gtsam {
}
// update with chart initializer, /todo: perhaps there is a way to init chart from old value...
template<typename ValueType, typename Chart, typename ChartInit>
void Values::update(Key j, const ValueType& val, ChartInit chart) {
template<typename ValueType, typename Chart>
void Values::update(Key j, const ValueType& val, Chart chart) {
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
}

8
gtsam/nonlinear/Values.h
View File

@ -263,8 +263,8 @@ namespace gtsam {
void insert(Key j, const ValueType& val);
/// overloaded insert version that also specifies a chart initializer
template <typename ValueType, typename Chart, typename ChartInit>
void insert(Key j, const ValueType& val, ChartInit chart);
template <typename ValueType, typename Chart>
void insert(Key j, const ValueType& val, Chart chart);
/** insert that mimics the STL map insert - if the value already exists, the map is not modified
@ -288,8 +288,8 @@ namespace gtsam {
void update(Key j, const T& val);
/// overloaded insert version that also specifies a chart initializer
template <typename T, typename Chart, typename ChartInit>
void update(Key j, const T& val, ChartInit chart);
template <typename T, typename Chart>
void update(Key j, const T& val, Chart chart);
/** update the current available values without adding new ones */
void update(const Values& values);

1
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -73,7 +73,6 @@ public:
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
if (H) {
const T value = expression_.value(x, keys_, dims_, *H);

87
gtsam_unstable/nonlinear/expressionTesting.h
View File

@ -30,19 +30,88 @@
namespace gtsam {
/**
* Linearize a nonlinear factor using numerical differentiation
* The benefit of this method is that it does not need to know what types are
* involved to evaluate the factor. If all the machinery of gtsam is working
* correctly, we should get the correct numerical derivatives out the other side.
*/
JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
const Values& values, double delta = 1e-5) {
// We will fill a map of Jacobians
std::map<Key, Matrix> jacobians;
// Get size
Eigen::VectorXd e = factor.whitenedError(values);
const size_t rows = e.size();
// Loop over all variables
VectorValues dX = values.zeroVectors();
BOOST_FOREACH(Key key, factor) {
// Compute central differences using the values struct.
const size_t cols = dX.dim(key);
Matrix J = Matrix::Zero(rows, cols);
for (size_t col = 0; col < cols; ++col) {
Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
dx[col] = delta;
dX[key] = dx;
Values eval_values = values.retract(dX);
Eigen::VectorXd left = factor.whitenedError(eval_values);
dx[col] = -delta;
dX[key] = dx;
eval_values = values.retract(dX);
Eigen::VectorXd right = factor.whitenedError(eval_values);
J.col(col) = (left - right) * (1.0 / (2.0 * delta));
}
jacobians[key] = J;
}
// Next step...return JacobianFactor
return JacobianFactor(jacobians, -e);
}
namespace internal {
// CPPUnitLite-style test for linearization of a factor
void testFactorJacobians(TestResult& result_, const std::string& name_,
const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
double tolerance) {
// Create expected value by numerical differentiation
JacobianFactor expected = linearizeNumerically(factor, values, delta);
// Create actual value by linearize
boost::shared_ptr<JacobianFactor> actual = //
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
// Check cast result and then equality
CHECK(actual);
EXPECT( assert_equal(expected, *actual, tolerance));
}
}
/// \brief Check the Jacobians produced by a factor against finite differences.
/// \param factor The factor to test.
/// \param values Values filled in for testing the Jacobians.
/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
/// \param tolerance The numerical tolerance to use when comparing Jacobians.
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
{ gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); }
namespace internal {
// CPPUnitLite-style test for linearization of an ExpressionFactor
template<typename T>
void testExpressionJacobians(TestResult& result_,
const std::string& name_,
const gtsam::Expression<T>& expression,
const gtsam::Values& values,
double nd_step,
double tolerance) {
void testExpressionJacobians(TestResult& result_, const std::string& name_,
const gtsam::Expression<T>& expression, const gtsam::Values& values,
double nd_step, double tolerance) {
// Create factor
size_t size = traits::dimension<T>::value;
ExpressionFactor<T> f(noiseModel::Unit::Create(size), expression.value(values), expression);
ExpressionFactor<T> f(noiseModel::Unit::Create(size),
expression.value(values), expression);
testFactorJacobians(result_, name_, f, values, nd_step, tolerance);
}
} // namespace gtsam
}
} // namespace gtsam
/// \brief Check the Jacobians produced by an expression against finite differences.
/// \param expression The expression to test.
@ -50,4 +119,4 @@ void testExpressionJacobians(TestResult& result_,
/// \param numerical_derivative_step The step to use when computing the finite difference Jacobians
/// \param tolerance The numerical tolerance to use when comparing Jacobians.
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \
{ gtsam::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); }
{ gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); }

118
gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp Normal file
View File

@ -0,0 +1,118 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------1------------------------------------------- */
/**
* @file testCustomChartExpression.cpp
* @date September 18, 2014
* @author Frank Dellaert
* @author Paul Furgale
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam_unstable/nonlinear/expressionTesting.h>
#include <gtsam/base/Testable.h>
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
// A simple prototype custom chart that does two things:
// 1. it reduces the dimension of the variable being estimated
// 2. it scales the input to retract.
//
// The Jacobian of this chart is:
// [ 2 0 ]
// [ 0 3 ]
// [ 0 0 ]
struct ProjectionChart {
typedef Eigen::Vector3d type;
typedef Eigen::Vector2d vector;
static vector local(const type& origin, const type& other) {
return (other - origin).head<2>();
}
static type retract(const type& origin, const vector& d) {
return origin + Eigen::Vector3d(2.0 * d[0], 3.0 * d[1], 0.0);
}
static int getDimension(const type& origin) {
return 2;
}
};
namespace gtsam {
namespace traits {
template<> struct is_chart<ProjectionChart> : public boost::true_type {};
template<> struct dimension<ProjectionChart> : public boost::integral_constant<int, 2> {};
} // namespace traits
} // namespace gtsam
TEST(ExpressionCustomChart, projection) {
// Create expression
Expression<Eigen::Vector3d> point(1);
Eigen::Vector3d pval(1.0, 2.0, 3.0);
Values standard;
standard.insert(1, pval);
Values custom;
custom.insert(1, pval, ProjectionChart());
Eigen::Vector3d pstandard = point.value(standard);
Eigen::Vector3d pcustom = point.value(custom);
// The values should be the same.
EXPECT(assert_equal(Matrix(pstandard), Matrix(pcustom), 1e-10));
EXPECT_LONGS_EQUAL(3, standard.at(1).dim());
VectorValues vstandard = standard.zeroVectors();
EXPECT_LONGS_EQUAL(3, vstandard.at(1).size());
EXPECT_LONGS_EQUAL(2, custom.at(1).dim());
VectorValues vcustom = custom.zeroVectors();
EXPECT_LONGS_EQUAL(2, vcustom.at(1).size());
ExpressionFactor<Eigen::Vector3d> f(noiseModel::Unit::Create(pval.size()), pval, point);
boost::shared_ptr<GaussianFactor> gfstandard = f.linearize(standard);
boost::shared_ptr<JacobianFactor> jfstandard = //
boost::dynamic_pointer_cast<JacobianFactor>(gfstandard);
typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
Jacobian Jstandard = jfstandard->jacobianUnweighted();
EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10));
boost::shared_ptr<GaussianFactor> gfcustom = f.linearize(custom);
boost::shared_ptr<JacobianFactor> jfcustom = //
boost::dynamic_pointer_cast<JacobianFactor>(gfcustom);
Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2);
expectedJacobian(0,0) = 2.0;
expectedJacobian(1,1) = 3.0;
Jacobian Jcustom = jfcustom->jacobianUnweighted();
EXPECT(assert_equal(expectedJacobian, Jcustom.first, 1e-10));
// Amazingly, the finite differences get the expected Jacobian right.
const double fd_step = 1e-5;
const double tolerance = 1e-5;
EXPECT_CORRECT_EXPRESSION_JACOBIANS(point, custom, fd_step, tolerance);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

175
tests/testNonlinearEquality.cpp
View File

@ -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);
}
//******************************************************************************