diff --git a/gtsam.h b/gtsam.h index 1fbc0f907..8c266b6a9 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1722,9 +1722,6 @@ 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); @@ -1742,12 +1739,6 @@ class Values { template T at(size_t j); - /// Fixed size versions, for n in 1..9 - void insertFixed(size_t j, Vector t, size_t n); - - /// Fixed size versions, for n in 1..9 - Vector atFixed(size_t j, size_t n); - /// version for double void insertDouble(size_t j, double c); double atDouble(size_t j) const; diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index fe2c3f3ca..96c352833 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -252,23 +252,97 @@ namespace gtsam { return filter(key_value.key); } - /* ************************************************************************* */ - template - const ValueType& Values::at(Key j) const { - // Find the item - KeyValueMap::const_iterator item = values_.find(j); + /* ************************************************************************* */ - // Throw exception if it does not exist - if(item == values_.end()) - throw ValuesKeyDoesNotExist("retrieve", j); + namespace internal { // Check the type and throw exception if incorrect - try { - return dynamic_cast&>(*item->second).value(); - } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); - } - } + template + struct handle { + ValueType operator()(Key j, const gtsam::Value * const pointer) { + try { + return dynamic_cast&>(*pointer).value(); + } catch (std::bad_cast &) { + throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); + } + } + }; + + // Handle dynamic vectors + template<> + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + throw ValuesIncorrectType(j, typeid(*pointer), + typeid(Eigen::Matrix)); + } + } + }; + + // Handle dynamic matrices + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + throw ValuesIncorrectType(j, typeid(*pointer), + typeid(Eigen::Matrix)); + } + } + }; + + // Request for a fixed vector + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + Matrix A = handle()(j, pointer); + if (A.rows() != M || A.cols() != 1) + throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols()); + else + return A; + } + } + }; + + // Request for a fixed matrix + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + Matrix A = handle()(j, pointer); + if (A.rows() != M || A.cols() != N) + throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); + else + return A; + } + } + }; + } // internal + + /* ************************************************************************* */ + template + ValueType Values::at(Key j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("at", j); + + return internal::handle()(j,item->second); + } /* ************************************************************************* */ template diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 372bced8e..ccbb51578 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -25,8 +25,6 @@ #include #include -#include - #include #ifdef __GNUC__ #pragma GCC diagnostic push @@ -38,6 +36,9 @@ #endif #include +#include +#include + using namespace std; namespace gtsam { @@ -112,24 +113,6 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - Vector Values::atFixed(Key j, size_t n) { - switch (n) { - case 1: return at(j); - case 2: return at(j); - case 3: return at(j); - case 4: return at(j); - case 5: return at(j); - case 6: return at(j); - case 7: return at(j); - case 8: return at(j); - case 9: return at(j); - default: - throw runtime_error( - "Values::at fixed size can only handle n in 1..9"); - } - } - /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item @@ -148,24 +131,6 @@ 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) { @@ -267,4 +232,18 @@ namespace gtsam { return message_.c_str(); } + /* ************************************************************************* */ + const char* NoMatchFoundForFixed::what() const throw() { + if(message_.empty()) { + ostringstream oss; + oss + << "Attempting to retrieve fixed-size matrix with dimensions " // + << M1_ << "x" << N1_ + << ", but found dynamic Matrix with mismatched dimensions " // + << M2_ << "x" << N2_; + message_ = oss.str(); + } + return message_.c_str(); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 73d0711be..15d3ac9e2 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -175,14 +175,10 @@ namespace gtsam { * @param j Retrieve the value associated with this key * @tparam Value The type of the value stored with this key, this method * throws DynamicValuesIncorrectType if this requested type is not correct. - * @return A const reference to the stored value + * @return The stored value */ template - const ValueType& at(Key j) const; - - /// Special version for small fixed size vectors, for matlab/python - /// throws truntime error if n<1 || n>9 - Vector atFixed(Key j, size_t n); + ValueType at(Key j) const; /// version for double double atDouble(size_t key) const { return at(key);} @@ -264,10 +260,6 @@ 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); - /// version for double void insertDouble(Key j, double c) { insert(j,c); } @@ -505,6 +497,28 @@ namespace gtsam { } }; + /* ************************************************************************* */ + class GTSAM_EXPORT NoMatchFoundForFixed: public std::exception { + + protected: + const size_t M1_, N1_; + const size_t M2_, N2_; + + private: + mutable std::string message_; + + public: + NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () : + M1_(M1), N1_(N1), M2_(M2), N2_(N2) { + } + + virtual ~NoMatchFoundForFixed() throw () { + } + + virtual const char* what() const throw (); + }; + + /* ************************************************************************* */ /// traits template<> struct traits : public Testable { diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index faa285cd5..ea5c67d9f 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -476,13 +476,22 @@ TEST(Values, Destructors) { } /* ************************************************************************* */ -TEST(Values, FixedSize) { +TEST(Values, FixedVector) { Values values; Vector v(3); v << 5.0, 6.0, 7.0; - values.insertFixed(key1, v, 3); + values.insert(key1, v); 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); + CHECK_EXCEPTION(values.at(key1), exception); +} +/* ************************************************************************* */ +TEST(Values, FixedMatrix) { + Values values; + Matrix v(1,3); v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector3 expected(5.0, 6.0, 7.0); + CHECK(assert_equal((Vector)expected, values.at(key1))); + CHECK_EXCEPTION(values.at(key1), exception); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }