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 22a2aefeb..273fb4c12 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -55,7 +55,7 @@ public: noiseModel_ = noiseModel; // Get dimensions of Jacobian matrices - // An Expression is assumed unmutable, so we do this now + // An Expression is assumed immutable, so we do this now std::map map; expression_.dims(map); size_t n = map.size(); @@ -83,12 +83,12 @@ 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) { // H should be pre-allocated assert(H->size()==size()); + // todo...fix for custom charts. VerticalBlockMatrix Ab(dimensions_, Dim); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ @@ -116,11 +116,20 @@ public: if (!active(x)) return boost::shared_ptr(); + // For custom charts, we would have to update the dimensions here + // based on the values that have been passed in. + FastVector::const_iterator kit = keys_.begin(); + std::vector dims; + dims.reserve(keys_.size()); + for(; kit != keys_.end(); ++kit) { + dims.push_back(x.at(*kit).dim()); + } + // Create a writeable JacobianFactor in advance // In case noise model is constrained, we need to provide a noise model bool constrained = noiseModel_->is_constrained(); boost::shared_ptr factor( - constrained ? new JacobianFactor(keys_, dimensions_, Dim, + constrained ? new JacobianFactor(keys_, dims, Dim, boost::static_pointer_cast(noiseModel_)->unit()) : new JacobianFactor(keys_, dimensions_, Dim)); diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp new file mode 100644 index 000000000..59df89b32 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * 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 testExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include + +#include + +#include +using boost::assign::list_of; +using boost::assign::map_list_of; + +using namespace std; +using namespace gtsam; + +// Dynamically sized Vector + +struct ProjectionChart { + typedef Eigen::Vector3d type; + typedef Eigen::Vector2d vector; + enum { dimension = 2 }; + 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) { + std::cout << "Hello!\n"; + // 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); +} +/* ************************************************************************* */ +