diff --git a/gtsam.h b/gtsam.h index 03963c773..c12055916 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1798,9 +1798,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); @@ -1818,12 +1815,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 0008252c4..446a67ec7 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -269,25 +269,114 @@ 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 - const Value& value = *item->second; - try { - return dynamic_cast&>(value).value(); - } catch (std::bad_cast &) { - // NOTE(abe): clang warns about potential side effects if done in typeid - const Value* value = item->second; - throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); - } + // Generic version, partially specialized below for various Eigen Matrix types + template + struct handle { + ValueType operator()(Key j, const gtsam::Value * const pointer) { + try { + // value returns a const ValueType&, and the return makes a copy !!!!! + 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 { + // value returns a const Vector&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // If a fixed vector was stored, we end up here as well. + 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 { + // value returns a const Matrix&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // If a fixed matrix was stored, we end up here as well. + throw ValuesIncorrectType(j, typeid(*pointer), + typeid(Eigen::Matrix)); + } + } + }; + + // Request for a fixed vector + // TODO(jing): is this piece of code really needed ??? + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const VectorM&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // Check if a dynamic vector was stored + Matrix A = handle()(j, pointer); // will throw if not.... + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != 1) + throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols()); + else + // This is not a copy because of RVO + return A; + } + } + }; + + // Request for a fixed matrix + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const MatrixMN&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // Check if a dynamic matrix was stored + Matrix A = handle()(j, pointer); // will throw if not.... + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != N) + throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); + else + // This is not a copy because of RVO + 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); + + // Check the type and throw exception if incorrect + return internal::handle()(j,item->second); } /* ************************************************************************* */ @@ -312,16 +401,48 @@ namespace gtsam { } /* ************************************************************************* */ + + // wrap all fixed in dynamics when insert and update + namespace internal { + + // general type + template + struct handle_wrap { + inline gtsam::GenericValue operator()(Key j, const ValueType& val) { + return gtsam::GenericValue(val); + } + }; + + // when insert/update a fixed size vector: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + // when insert/update a fixed size matrix: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + } + // insert a templated value template - void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(GenericValue(val))); - } + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(internal::handle_wrap()(j, val))); + } // update with templated value template void Values::update(Key j, const ValueType& val) { - update(j, static_cast(GenericValue(val))); + update(j, static_cast(internal::handle_wrap()(j, val))); } } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 07757062c..ba7a040cd 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) { @@ -269,4 +234,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 70aadfa06..a61d01f23 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -168,16 +168,13 @@ namespace gtsam { /** Retrieve a variable by key \c j. The type of the value associated with * this key is supplied as a template argument to this function. * @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 + * @tparam ValueType The type of the value stored with this key, this method + * Throws DynamicValuesIncorrectType if this requested type is not correct. + * Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa. + * @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);} @@ -259,10 +256,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); } @@ -500,6 +493,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/expressions.h b/gtsam/nonlinear/expressions.h index 5b591bcf0..19fd257e8 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -25,7 +25,7 @@ Expression compose(const Expression& t1, const Expression& t2) { } // Some typedefs -typedef Expression double_; +typedef Expression Double_; typedef Expression Vector1_; typedef Expression Vector2_; typedef Expression Vector3_; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 43f1cd017..8f059f84c 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -17,7 +17,7 @@ * @brief unit tests for Block Automatic Differentiation */ -#include +#include #include #include #include @@ -32,9 +32,7 @@ using boost::assign::map_list_of; using namespace std; using namespace gtsam; -typedef Expression double_; typedef Expression Point3_; -typedef Expression Vector3_; typedef Expression Pose3_; typedef Expression Rot3_; @@ -101,7 +99,7 @@ TEST(Expression, Unary1) { } TEST(Expression, Unary2) { using namespace unary; - double_ e(f2, p); + Double_ e(f2, p); EXPECT(expected == e.keys()); } @@ -156,7 +154,7 @@ Point3_ p_cam(x, &Pose3::transform_to, p); // Check that creating an expression to double compiles TEST(Expression, BinaryToDouble) { using namespace binary; - double_ p_cam(doubleF, x, p); + Double_ p_cam(doubleF, x, p); } /* ************************************************************************* */ @@ -372,8 +370,8 @@ TEST(Expression, TripleSum) { /* ************************************************************************* */ TEST(Expression, SumOfUnaries) { const Key key(67); - const double_ norm_(>sam::norm, Point3_(key)); - const double_ sum_ = norm_ + norm_; + const Double_ norm_(>sam::norm, Point3_(key)); + const Double_ sum_ = norm_ + norm_; Values values; values.insert(key, Point3(6, 0, 0)); @@ -391,7 +389,7 @@ TEST(Expression, SumOfUnaries) { TEST(Expression, UnaryOfSum) { const Key key1(42), key2(67); const Point3_ sum_ = Point3_(key1) + Point3_(key2); - const double_ norm_(>sam::norm, sum_); + const Double_ norm_(>sam::norm, sum_); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); norm_.dims(actual_dims); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index dfc14e1e6..8c1c85038 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -477,13 +477,59 @@ TEST(Values, Destructors) { } /* ************************************************************************* */ -TEST(Values, FixedSize) { +TEST(Values, VectorDynamicInsertFixedRead) { 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); + Vector3 actual = values.at(key1); + CHECK(assert_equal(expected, actual)); + CHECK_EXCEPTION(values.at(key1), exception); +} + +/* ************************************************************************* */ +TEST(Values, VectorDynamicInsertDynamicRead) { + Values values; + Vector v(3); v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector expected(3); expected << 5.0, 6.0, 7.0; + Vector actual = values.at(key1); + LONGS_EQUAL(3, actual.rows()); + LONGS_EQUAL(1, actual.cols()); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Values, VectorFixedInsertFixedRead) { + Values values; + Vector3 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector3 expected; expected << 5.0, 6.0, 7.0; + Vector3 actual = values.at(key1); + CHECK(assert_equal(expected, actual)); + CHECK_EXCEPTION(values.at(key1), exception); +} + +/* ************************************************************************* */ +TEST(Values, VectorFixedInsertDynamicRead) { + Values values; + Vector3 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector expected(3); expected << 5.0, 6.0, 7.0; + Vector actual = values.at(key1); + LONGS_EQUAL(3, actual.rows()); + LONGS_EQUAL(1, actual.cols()); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Values, MatrixDynamicInsertFixedRead) { + 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); } diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index b500b36e3..e37e8ff20 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -17,7 +17,7 @@ * @date December 2014 */ -#include +#include #include namespace gtsam { @@ -25,7 +25,7 @@ namespace gtsam { /// A "Time of Arrival" factor - so little code seems hardly worth it :-) class TOAFactor: public ExpressionFactor { - typedef Expression double_; + typedef Expression Double_; public: @@ -40,7 +40,7 @@ public: const Expression& microphone_, double toaMeasurement, const SharedNoiseModel& model) : ExpressionFactor(model, toaMeasurement, - double_(&Event::toa, eventExpression, microphone_)) { + Double_(&Event::toa, eventExpression, microphone_)) { } }; diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 52786702d..233dbdceb 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -31,7 +31,6 @@ using namespace std; using namespace gtsam; // typedefs -typedef Eigen::Matrix Vector1; typedef Expression Point3_; typedef Expression Event_; @@ -52,7 +51,7 @@ TEST( TOAFactor, NewWay ) { Event_ eventExpression(key); Point3_ microphoneConstant(microphoneAt0); // constant expression double measurement = 7; - double_ expression(&Event::toa, eventExpression, microphoneConstant); + Double_ expression(&Event::toa, eventExpression, microphoneConstant); ExpressionFactor factor(model, measurement, expression); } @@ -92,7 +91,7 @@ TEST( TOAFactor, WholeEnchilada ) { Event_ eventExpression(key); for (size_t i = 0; i < K; i++) { Point3_ microphone_i(microphones[i]); // constant expression - double_ predictTOA(&Event::toa, eventExpression, microphone_i); + Double_ predictTOA(&Event::toa, eventExpression, microphone_i); graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); } diff --git a/matlab/+gtsam/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m index 46e161807..20931499b 100644 --- a/matlab/+gtsam/plot3DPoints.m +++ b/matlab/+gtsam/plot3DPoints.m @@ -7,7 +7,7 @@ function plot3DPoints(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'g'; + linespec = 'g*'; end haveMarginals = exist('marginals', 'var'); keys = KeyVector(values.keys); @@ -15,19 +15,25 @@ keys = KeyVector(values.keys); holdstate = ishold; hold on -% Plot points and covariance matrices -for i = 0:keys.size-1 - key = keys.at(i); - try - p = values.atPoint3(key); - if haveMarginals +if haveMarginals + % Plot points and covariance matrices (slow) + for i = 0:keys.size-1 + key = keys.at(i); + try + p = values.atPoint3(key) P = marginals.marginalCovariance(key); gtsam.plotPoint3(p, linespec, P); - else - gtsam.plotPoint3(p, linespec); + catch + % I guess it's not a Point3 end - catch - % I guess it's not a Point3 + end +else + % Extract all in C++ and plot all at once (fast) + P = utilities.extractPoint3(values); + if size(linespec,2)==1 + plot3(P(:,1),P(:,2),P(:,3),[linespec '*']); + else + plot3(P(:,1),P(:,2),P(:,3),linespec); end end diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index b635589c3..0d1349972 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -56,7 +56,6 @@ void exportValues(){ .def("empty", &Values::empty) .def("equals", &Values::equals) .def("erase", &Values::erase) - .def("insert_fixed", &Values::insertFixed) .def("print", &Values::print, print_overloads(args("s"))) .def("size", &Values::size) .def("swap", &Values::swap) @@ -69,15 +68,15 @@ void exportValues(){ .def("insert", insert_bias) .def("insert", insert_navstate) .def("insert", insert_vector3) - .def("atPoint2", &Values::at, return_value_policy()) - .def("atRot2", &Values::at, return_value_policy()) - .def("atPose2", &Values::at, return_value_policy()) - .def("atPoint3", &Values::at, return_value_policy()) - .def("atRot3", &Values::at, return_value_policy()) - .def("atPose3", &Values::at, return_value_policy()) - .def("atConstantBias", &Values::at, return_value_policy()) - .def("atNavState", &Values::at, return_value_policy()) - .def("atVector3", &Values::at, return_value_policy()) + .def("atPoint2", &Values::at) + .def("atRot2", &Values::at) + .def("atPose2", &Values::at) + .def("atPoint3", &Values::at) + .def("atRot3", &Values::at) + .def("atPose3", &Values::at) + .def("atConstantBias", &Values::at) + .def("atNavState", &Values::at) + .def("atVector3", &Values::at) .def("exists", exists1) .def("keys", &Values::keys) ;