diff --git a/.cproject b/.cproject
index 1dcc51dfe..d42d2b0e0 100644
--- a/.cproject
+++ b/.cproject
@@ -2777,6 +2777,14 @@
true
true
+
+ make
+ -j4
+ testCustomChartExpression.run
+ true
+ true
+ true
+
make
-j2
diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index 30136d9f2..f2c4566bb 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -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
-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 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
-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 gf = f.linearize(values);
- boost::shared_ptr jf = //
- boost::dynamic_pointer_cast(gf);
-
- typedef std::pair 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); }
-
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index 5d3319d97..9e3230f34 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -26,280 +26,311 @@
namespace gtsam {
+/**
+ * Template default compare function that assumes a testable T
+ */
+template
+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 NonlinearEquality: public NoiseModelFactor1 {
+
+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 This;
+
+ // typedef to base class
+ typedef NoiseModelFactor1 Base;
+
+public:
+
/**
- * Template default compare function that assumes a testable T
+ * Function that compares two values
*/
- template
- 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) :
+ 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 NonlinearEquality: public NoiseModelFactor1 {
+ NonlinearEquality(Key j, const T& feasible, double error_gain,
+ bool (*_compare)(const T&, const T&) = compare) :
+ 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(&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 This;
-
- // typedef to base class
- typedef NoiseModelFactor1 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) :
- 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(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) :
- 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 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::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(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::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(&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(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
+ void serialize(ARCHIVE & ar, const unsigned int version) {
+ ar
+ & boost::serialization::make_nvp("NoiseModelFactor1",
+ boost::serialization::base_object(*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 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::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(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 NonlinearEquality1: public NoiseModelFactor1 {
- /// @return a deep copy of this factor
- virtual gtsam::NonlinearFactor::shared_ptr clone() const {
- return boost::static_pointer_cast(
- gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+public:
+ typedef VALUE X;
- /// @}
+protected:
+ typedef NoiseModelFactor1 Base;
+ typedef NonlinearEquality1 This;
- private:
+ /** default constructor to allow for serialization */
+ NonlinearEquality1() {
+ }
- /** Serialization function */
- friend class boost::serialization::access;
- template
- void serialize(ARCHIVE & ar, const unsigned int version) {
- ar & boost::serialization::make_nvp("NoiseModelFactor1",
- boost::serialization::base_object(*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 NonlinearEquality1 : public NoiseModelFactor1 {
+public:
- public:
- typedef VALUE X;
+ typedef boost::shared_ptr > shared_ptr;
- protected:
- typedef NoiseModelFactor1 Base;
- typedef NonlinearEquality1 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::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 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 > 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
+ void serialize(ARCHIVE & ar, const unsigned int version) {
+ ar
+ & boost::serialization::make_nvp("NoiseModelFactor1",
+ boost::serialization::base_object(*this));
+ ar & BOOST_SERIALIZATION_NVP(value_);
+ }
+};
+// \NonlinearEquality1
- virtual ~NonlinearEquality1() {}
+/* ************************************************************************* */
+/**
+ * Simple binary equality constraint - this constraint forces two factors to
+ * be the same.
+ */
+template
+class NonlinearEquality2: public NoiseModelFactor2 {
+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::shared_ptr(new This(*this))); }
+protected:
+ typedef NoiseModelFactor2 Base;
+ typedef NonlinearEquality2 This;
- /** g(x) with optional derivative */
- Vector evaluateError(const X& x1, boost::optional 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
- void serialize(ARCHIVE & ar, const unsigned int version) {
- ar & boost::serialization::make_nvp("NoiseModelFactor1",
- boost::serialization::base_object(*this));
- ar & BOOST_SERIALIZATION_NVP(value_);
- }
- }; // \NonlinearEquality1
+ typedef boost::shared_ptr > shared_ptr;
- /* ************************************************************************* */
- /**
- * Simple binary equality constraint - this constraint forces two factors to
- * be the same.
- */
- template
- class NonlinearEquality2 : public NoiseModelFactor2 {
- 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 Base;
- typedef NonlinearEquality2 This;
+ /// @return a deep copy of this factor
+ virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+ return boost::static_pointer_cast(
+ 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 H1 =
+ boost::none, boost::optional 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
+ void serialize(ARCHIVE & ar, const unsigned int version) {
+ ar
+ & boost::serialization::make_nvp("NoiseModelFactor2",
+ boost::serialization::base_object(*this));
+ }
+};
+// \NonlinearEquality2
- typedef boost::shared_ptr > 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::shared_ptr(new This(*this))); }
-
- /** g(x) with optional derivative2 */
- Vector evaluateError(const X& x1, const X& x2,
- boost::optional H1 = boost::none,
- boost::optional 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
- void serialize(ARCHIVE & ar, const unsigned int version) {
- ar & boost::serialization::make_nvp("NoiseModelFactor2",
- boost::serialization::base_object(*this));
- }
- }; // \NonlinearEquality2
-
-} // namespace gtsam
+}// namespace gtsam
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 4595a70ed..e983ccb17 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -302,8 +302,8 @@ namespace gtsam {
}
// overloaded insert with chart initializer
- template
- void Values::insert(Key j, const ValueType& val, ChartInit chart) {
+ template
+ void Values::insert(Key j, const ValueType& val, Chart chart) {
insert(j, static_cast(ChartValue(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
- void Values::update(Key j, const ValueType& val, ChartInit chart) {
+ template
+ void Values::update(Key j, const ValueType& val, Chart chart) {
update(j, static_cast(ChartValue(val, chart)));
}
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index e4a27849d..d00baa0d9 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -263,8 +263,8 @@ namespace gtsam {
void insert(Key j, const ValueType& val);
/// overloaded insert version that also specifies a chart initializer
- template
- void insert(Key j, const ValueType& val, ChartInit chart);
+ template
+ 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
- void update(Key j, const T& val, ChartInit chart);
+ template
+ void update(Key j, const T& val, Chart chart);
/** update the current available values without adding new ones */
void update(const Values& values);
diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index 069fb5e2e..181ac49b4 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -73,7 +73,6 @@ public:
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional&> H = boost::none) const {
- // TODO(PTF) Is this a place for custom charts?
DefaultChart chart;
if (H) {
const T value = expression_.value(x, keys_, dims_, *H);
diff --git a/gtsam_unstable/nonlinear/expressionTesting.h b/gtsam_unstable/nonlinear/expressionTesting.h
index ae2c1e7e5..92c8f71e8 100644
--- a/gtsam_unstable/nonlinear/expressionTesting.h
+++ b/gtsam_unstable/nonlinear/expressionTesting.h
@@ -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 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 actual = //
+ boost::dynamic_pointer_cast(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
-void testExpressionJacobians(TestResult& result_,
- const std::string& name_,
- const gtsam::Expression& expression,
- const gtsam::Values& values,
- double nd_step,
- double tolerance) {
+void testExpressionJacobians(TestResult& result_, const std::string& name_,
+ const gtsam::Expression& expression, const gtsam::Values& values,
+ double nd_step, double tolerance) {
// Create factor
size_t size = traits::dimension::value;
- ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression);
+ ExpressionFactor 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); }
diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp
new file mode 100644
index 000000000..bc2055c55
--- /dev/null
+++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp
@@ -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
+#include
+#include
+#include
+#include
+
+#include
+
+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 : public boost::true_type {};
+template<> struct dimension : public boost::integral_constant {};
+} // namespace traits
+} // namespace gtsam
+
+TEST(ExpressionCustomChart, projection) {
+ // Create expression
+ Expression 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 f(noiseModel::Unit::Create(pval.size()), pval, point);
+
+ boost::shared_ptr gfstandard = f.linearize(standard);
+ boost::shared_ptr jfstandard = //
+ boost::dynamic_pointer_cast(gfstandard);
+
+ typedef std::pair Jacobian;
+ Jacobian Jstandard = jfstandard->jacobianUnweighted();
+ EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10));
+
+ boost::shared_ptr gfcustom = f.linearize(custom);
+ boost::shared_ptr jfcustom = //
+ boost::dynamic_pointer_cast(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);
+}
+/* ************************************************************************* */
+
diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp
index 6366d9fa5..96039a3cc 100644
--- a/tests/testNonlinearEquality.cpp
+++ b/tests/testNonlinearEquality.cpp
@@ -42,9 +42,9 @@ typedef PriorFactor PosePrior;
typedef NonlinearEquality PoseNLE;
typedef boost::shared_ptr 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::infinity())));
+ EXPECT(
+ assert_equal(actual, repeat(3, std::numeric_limits::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 shK(new Cal3_S2(K));
// typedefs for visual SLAM example
@@ -516,14 +526,12 @@ typedef NonlinearFactorGraph VGraph;
// factors for visual slam
typedef NonlinearEquality2 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(camera1.project(landmark), vmodel, x1, l1, shK);
- graph += GenericProjectionFactor(camera2.project(landmark), vmodel, x2, l2, shK);
+ graph += GenericProjectionFactor(
+ camera1.project(landmark), vmodel, x1, l1, shK);
+ graph += GenericProjectionFactor(
+ 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);
+}
+//******************************************************************************