diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 078ffc502..0436ade4a 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -295,10 +295,11 @@ namespace gtsam { template struct handle_matrix, true> { inline Eigen::Matrix operator()(Key j, const Value* const pointer) { - try { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { // value returns a const Matrix&, and the return makes a copy !!!!! - return dynamic_cast>&>(*pointer).value(); - } catch (std::bad_cast&) { + return ptr->value(); + } else { // If a fixed matrix was stored, we end up here as well. throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); } @@ -312,13 +313,13 @@ namespace gtsam { auto ptr = dynamic_cast>*>(pointer); if (ptr) { // value returns a const MatrixMN&, and the return makes a copy !!!!! - return dynamic_cast>&>(*pointer).value(); + return ptr->value(); } else { Matrix A; + // Check if a dynamic matrix was stored auto ptr = dynamic_cast*>(pointer); if (ptr) { - // Check if a dynamic matrix was stored - A = ptr->value(); // will throw if not.... + A = ptr->value(); } else { // Or a dynamic vector A = handle_matrix()(j, pointer); // will throw if not....