diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 8a905a0ac..90ede7a2d 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -261,6 +261,7 @@ namespace gtsam { 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)); @@ -274,42 +275,49 @@ namespace gtsam { 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 &) { - Matrix A = handle()(j, pointer); + // 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; } } @@ -321,12 +329,16 @@ namespace gtsam { 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 &) { - Matrix A = handle()(j, pointer); + // 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; } } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 15d3ac9e2..780a75931 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -173,8 +173,9 @@ 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. + * @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