diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index dfcb7e174..078ffc502 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -294,7 +294,7 @@ namespace gtsam { // Handle dynamic matrices template struct handle_matrix, true> { - Eigen::Matrix operator()(Key j, const Value* const pointer) { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { try { // value returns a const Matrix&, and the return makes a copy !!!!! return dynamic_cast>&>(*pointer).value(); @@ -308,16 +308,18 @@ namespace gtsam { // Handle fixed matrices template struct handle_matrix, false> { - Eigen::Matrix operator()(Key j, const Value* const pointer) { - try { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { // value returns a const MatrixMN&, and the return makes a copy !!!!! return dynamic_cast>&>(*pointer).value(); - } catch (std::bad_cast&) { + } else { Matrix A; - try { + auto ptr = dynamic_cast*>(pointer); + if (ptr) { // Check if a dynamic matrix was stored - A = handle_matrix()(j, pointer); // will throw if not.... - } catch (const ValuesIncorrectType&) { + A = ptr->value(); // will throw if not.... + } else { // Or a dynamic vector A = handle_matrix()(j, pointer); // will throw if not.... }