From 22218eafc478db348ca37dabc643b2338fa5c4e0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 10:43:08 +0100 Subject: [PATCH] We will not wrap fixed Vectors and Matrices individually. Methods that take those can just as well be wrapped with Vector or Matrix. Note const and & are ignored for those. I added a small method, insertFixed, that allows one to put in small fixed size matrices for optimization. Did not do retrieval yet. --- gtsam.h | 18 ++++++++---------- gtsam/nonlinear/Values.cpp | 18 ++++++++++++++++++ gtsam/nonlinear/Values.h | 4 ++++ gtsam/nonlinear/tests/testValues.cpp | 9 +++++++++ gtsam_unstable/nonlinear/ExpressionFactor.h | 2 ++ 5 files changed, 41 insertions(+), 10 deletions(-) diff --git a/gtsam.h b/gtsam.h index 67b3f2888..56bf378b5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,12 +156,6 @@ virtual class Value { size_t dim() const; }; -class Vector3 { - Vector3(Vector v); -}; -class Vector6 { - Vector6(Vector v); -}; #include class LieScalar { // Standard constructors @@ -1723,6 +1717,7 @@ class Values { // void insert(size_t j, const gtsam::Value& value); // void update(size_t j, const gtsam::Value& val); // gtsam::Value at(size_t j) const; + void insert(size_t j, const gtsam::Point2& t); void insert(size_t j, const gtsam::Point3& t); void insert(size_t j, const gtsam::Rot2& t); @@ -1737,6 +1732,9 @@ class Values { void insert(size_t j, Vector t); void insert(size_t j, Matrix t); + // Fixed size version + void insertFixed(size_t j, Vector t, size_t n); + void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); void update(size_t j, const gtsam::Rot2& t); @@ -2394,7 +2392,7 @@ class ConstantBias { #include class PoseVelocity{ - PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity); + PoseVelocity(const gtsam::Pose3& pose, Vector velocity); }; class ImuFactorPreintegratedMeasurements { // Standard Constructor @@ -2433,7 +2431,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor); // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias, + gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis) const; }; @@ -2479,7 +2477,7 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { #include class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias); + PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); }; class CombinedImuFactorPreintegratedMeasurements { // Standard Constructor @@ -2530,7 +2528,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i, + gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); }; diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index b92e54143..3cad10f31 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -130,6 +130,24 @@ namespace gtsam { throw ValuesKeyAlreadyExists(j); } + /* ************************************************************************* */ + void Values::insertFixed(Key j, const Vector& v, size_t n) { + switch (n) { + case 1: insert(j,v); break; + case 2: insert(j,v); break; + case 3: insert(j,v); break; + case 4: insert(j,v); break; + case 5: insert(j,v); break; + case 6: insert(j,v); break; + case 7: insert(j,v); break; + case 8: insert(j,v); break; + case 9: insert(j,v); break; + default: + throw runtime_error( + "Values::insert fixed size can only handle n in 1..9"); + } + } + /* ************************************************************************* */ void Values::insert(const Values& values) { for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index d00baa0d9..9a4c7e798 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -258,6 +258,10 @@ namespace gtsam { template void insert(Key j, const ValueType& val); + /// Special version for small fixed size vectors, for matlab/python + /// throws truntime error if n<1 || n>9 + void insertFixed(Key j, const Vector& v, size_t n); + /// overloaded insert version that also specifies a chart template void insert(Key j, const ValueType& val); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 941728d8c..1b1d76d8a 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -470,6 +470,15 @@ TEST(Values, Destructors) { LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount); } +/* ************************************************************************* */ +TEST(Values, FixedSize) { + Values values; + Vector v(3); v << 5.0, 6.0, 7.0; + values.insertFixed(key1, v, 3); + Vector3 expected(5.0, 6.0, 7.0); + CHECK(assert_equal((Vector)expected, values.at(key1))); + CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index de6e4983a..56e72a807 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -32,6 +32,8 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { +protected: + T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled FastVector dims_; ///< dimensions of the Jacobian matrices