12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1168 from borglab/fix/slow_cast 

Fix slow dynamic cast
release/4.3a0
Fan Jiang 2022-04-14 14:57:23 -04:00 committed by GitHub
parent caa14bc5aa 1b63fb51eb
commit 415e41b0f5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 27 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam/nonlinear/Values-inl.h
View File

@ -279,10 +279,11 @@ namespace gtsam {
template <typename ValueType> template <typename ValueType>
struct handle { struct handle {
ValueType operator()(Key j, const Value* const pointer) { ValueType operator()(Key j, const Value* const pointer) {
try { auto ptr = dynamic_cast<const GenericValue<ValueType>*>(pointer);
if (ptr) {
// value returns a const ValueType&, and the return makes a copy !!!!! // value returns a const ValueType&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value(); return ptr->value();
} catch (std::bad_cast&) { } else {
throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
} }
} }
@ -294,11 +295,12 @@ namespace gtsam {
// Handle dynamic matrices // Handle dynamic matrices
template <int M, int N> template <int M, int N>
struct handle_matrix<Eigen::Matrix<double, M, N>, true> { struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) { inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
try { auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
if (ptr) {
// value returns a const Matrix&, and the return makes a copy !!!!! // value returns a const Matrix&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value(); return ptr->value();
} catch (std::bad_cast&) { } else {
// If a fixed matrix was stored, we end up here as well. // If a fixed matrix was stored, we end up here as well.
throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>)); throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
} }
@ -308,16 +310,18 @@ namespace gtsam {
// Handle fixed matrices // Handle fixed matrices
template <int M, int N> template <int M, int N>
struct handle_matrix<Eigen::Matrix<double, M, N>, false> { struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) { inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
try { auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
if (ptr) {
// value returns a const MatrixMN&, and the return makes a copy !!!!! // value returns a const MatrixMN&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value(); return ptr->value();
} catch (std::bad_cast&) { } else {
Matrix A; Matrix A;
try {
// Check if a dynamic matrix was stored // Check if a dynamic matrix was stored
A = handle_matrix<Eigen::MatrixXd, true>()(j, pointer); // will throw if not.... auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
} catch (const ValuesIncorrectType&) { if (ptr) {
A = ptr->value();
} else {
// Or a dynamic vector // Or a dynamic vector
A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not.... A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
} }
@ -364,10 +368,10 @@ namespace gtsam {
if(item != values_.end()) { if(item != values_.end()) {
// dynamic cast the type and throw exception if incorrect // dynamic cast the type and throw exception if incorrect
const Value& value = *item->second; auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
try { if (ptr) {
return dynamic_cast<const GenericValue<ValueType>&>(value).value(); return ptr->value();
} catch (std::bad_cast &) { } else {
// NOTE(abe): clang warns about potential side effects if done in typeid // NOTE(abe): clang warns about potential side effects if done in typeid
const Value* value = item->second; const Value* value = item->second;
throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));

4
gtsam/nonlinear/nonlinear.i
View File

@ -245,6 +245,10 @@ class Values {
void insert(size_t j, const gtsam::ParameterMatrix<14>& X); void insert(size_t j, const gtsam::ParameterMatrix<14>& X);
void insert(size_t j, const gtsam::ParameterMatrix<15>& X); void insert(size_t j, const gtsam::ParameterMatrix<15>& X);
template <T = {gtsam::Point2,
gtsam::Point3}>
void insert(size_t j, const T& val);
void update(size_t j, const gtsam::Point2& point2); 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::Point3& point3);
void update(size_t j, const gtsam::Rot2& rot2); void update(size_t j, const gtsam::Rot2& rot2);