diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index dfcb7e174..0370c5cee 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -279,10 +279,11 @@ namespace gtsam { template struct handle { ValueType operator()(Key j, const Value* const pointer) { - try { + auto ptr = dynamic_cast*>(pointer); + if (ptr) { // value returns a const ValueType&, and the return makes a copy !!!!! - return dynamic_cast&>(*pointer).value(); - } catch (std::bad_cast&) { + return ptr->value(); + } else { throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); } } @@ -294,11 +295,12 @@ namespace gtsam { // Handle dynamic matrices template struct handle_matrix, true> { - 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 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)); } @@ -308,16 +310,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&) { + return ptr->value(); + } else { Matrix A; - try { - // Check if a dynamic matrix was stored - A = handle_matrix()(j, pointer); // will throw if not.... - } catch (const ValuesIncorrectType&) { + // Check if a dynamic matrix was stored + auto ptr = dynamic_cast*>(pointer); + if (ptr) { + A = ptr->value(); + } else { // Or a dynamic vector A = handle_matrix()(j, pointer); // will throw if not.... } @@ -364,10 +368,10 @@ namespace gtsam { if(item != values_.end()) { // dynamic cast the type and throw exception if incorrect - const Value& value = *item->second; - try { - return dynamic_cast&>(value).value(); - } catch (std::bad_cast &) { + auto ptr = dynamic_cast*>(item->second); + if (ptr) { + return ptr->value(); + } else { // NOTE(abe): clang warns about potential side effects if done in typeid const Value* value = item->second; throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 326b84d16..3fff71978 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -245,6 +245,10 @@ class Values { void insert(size_t j, const gtsam::ParameterMatrix<14>& X); void insert(size_t j, const gtsam::ParameterMatrix<15>& X); + template + void insert(size_t j, const T& val); + void update(size_t j, const gtsam::Point2& point2); void update(size_t j, const gtsam::Point3& point3); void update(size_t j, const gtsam::Rot2& rot2);