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> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <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); 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 } // 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); }

131
gtsam/nonlinear/NonlinearEquality.h
View File

@ -77,11 +77,12 @@ namespace gtsam {
*/ */
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,26 +90,28 @@ 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;
@ -117,8 +120,8 @@ namespace gtsam {
/** 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;
} }
/// @} /// @}
@ -137,17 +140,22 @@ namespace gtsam {
} }
/** 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,13 +166,15 @@ 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)));
}
/// @} /// @}
@ -174,14 +184,16 @@ namespace gtsam {
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
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
@ -198,39 +210,48 @@ namespace gtsam {
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");
} }
@ -241,11 +262,13 @@ namespace gtsam {
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
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
@ -261,32 +284,38 @@ namespace gtsam {
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);
} }
@ -296,10 +325,12 @@ namespace gtsam {
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

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

@ -302,8 +302,8 @@ namespace gtsam {
} }
// overloaded insert with chart initializer // overloaded insert with chart initializer
template<typename ValueType, typename Chart, typename ChartInit> template<typename ValueType, typename Chart>
void Values::insert(Key j, const ValueType& val, ChartInit chart) { void Values::insert(Key j, const ValueType& val, Chart chart) {
insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, 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... // update with chart initializer, /todo: perhaps there is a way to init chart from old value...
template<typename ValueType, typename Chart, typename ChartInit> template<typename ValueType, typename Chart>
void Values::update(Key j, const ValueType& val, ChartInit chart) { void Values::update(Key j, const ValueType& val, Chart chart) {
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, 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); void insert(Key j, const ValueType& val);
/// overloaded insert version that also specifies a chart initializer /// overloaded insert version that also specifies a chart initializer
template <typename ValueType, typename Chart, typename ChartInit> template <typename ValueType, typename Chart>
void insert(Key j, const ValueType& val, ChartInit 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 /** 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); void update(Key j, const T& val);
/// overloaded insert version that also specifies a chart initializer /// overloaded insert version that also specifies a chart initializer
template <typename T, typename Chart, typename ChartInit> template <typename T, typename Chart>
void update(Key j, const T& val, ChartInit chart); void update(Key j, const T& val, Chart chart);
/** update the current available values without adding new ones */ /** update the current available values without adding new ones */
void update(const Values& values); void update(const Values& values);

1
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

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

85
gtsam_unstable/nonlinear/expressionTesting.h
View File

@ -30,18 +30,87 @@
namespace gtsam { namespace gtsam {
template<typename T> /**
void testExpressionJacobians(TestResult& result_, * Linearize a nonlinear factor using numerical differentiation
const std::string& name_, * The benefit of this method is that it does not need to know what types are
const gtsam::Expression<T>& expression, * involved to evaluate the factor. If all the machinery of gtsam is working
const gtsam::Values& values, * correctly, we should get the correct numerical derivatives out the other side.
double nd_step, */
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) { 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) {
// Create factor // Create factor
size_t size = traits::dimension<T>::value; 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); testFactorJacobians(result_, name_, f, values, nd_step, tolerance);
} }
}
} // namespace gtsam } // namespace gtsam
/// \brief Check the Jacobians produced by an expression against finite differences. /// \brief Check the Jacobians produced by an expression against finite differences.
@ -50,4 +119,4 @@ void testExpressionJacobians(TestResult& result_,
/// \param numerical_derivative_step The step to use when computing the finite difference Jacobians /// \param numerical_derivative_step The step to use when computing the finite difference Jacobians
/// \param tolerance The numerical tolerance to use when comparing Jacobians. /// \param tolerance The numerical tolerance to use when comparing Jacobians.
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \ #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);
}
/* ************************************************************************* */

121
tests/testNonlinearEquality.cpp
View File

@ -44,7 +44,7 @@ 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,7 +60,7 @@ 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);
@ -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,12 +150,15 @@ 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);
@ -180,11 +182,12 @@ TEST ( NonlinearEquality, allow_error_pose ) {
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);
@ -211,7 +214,7 @@ 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
@ -245,11 +248,11 @@ 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);
@ -267,12 +270,14 @@ 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);
@ -282,18 +287,20 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
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
@ -326,7 +333,7 @@ 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);
@ -347,12 +354,14 @@ 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);
@ -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,12 +384,12 @@ 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
@ -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
@ -430,8 +439,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
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);
@ -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,7 +469,7 @@ 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;
@ -494,7 +503,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
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));
@ -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
@ -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);
}
//******************************************************************************