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

Fix slow dynamic cast

release/4.3a0
Fan Jiang 2022-04-12 13:56:36 -04:00 committed by GitHub
parent d3162bfb5e
commit 49b40d3942
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -294,7 +294,7 @@ 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 { try {
// 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 dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
@ -308,16 +308,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 dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
} catch (std::bad_cast&) { } else {
Matrix A; Matrix A;
try { auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
if (ptr) {
// 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.... A = ptr->value(); // will throw if not....
} catch (const ValuesIncorrectType&) { } 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....
} }