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

Temporarily reverted maximum interoperability with MATLAB as it very much slowed down Values with losts of fixed Vectors on the C++ side

release/4.3a0
Frank Dellaert 2016-05-09 13:48:59 -07:00
parent 71d73f283a
commit 8826666224
2 changed files with 74 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -271,99 +271,74 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
namespace internal { namespace internal {
// Check the type and throw exception if incorrect // Check the type and throw exception if incorrect
// Generic version, partially specialized below for various Eigen Matrix types // Generic version, partially specialized below for various Eigen Matrix types
template<typename ValueType> template <typename ValueType>
struct handle { struct handle {
ValueType operator()(Key j, const gtsam::Value * const pointer) { ValueType operator()(Key j, const Value* const pointer) {
try { try {
// 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 dynamic_cast<const GenericValue<ValueType>&>(*pointer).value();
} catch (std::bad_cast &) { } catch (std::bad_cast&) {
throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
} }
} }
}; };
// Handle dynamic vectors template <typename MatrixType, bool isDynamic>
template<> struct handle_matrix;
struct handle<Eigen::Matrix<double, -1, 1> > {
Eigen::Matrix<double, -1, 1> operator()(Key j,
const gtsam::Value * const pointer) {
try {
// value returns a const Vector&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
} catch (std::bad_cast &) {
// If a fixed vector was stored, we end up here as well.
throw ValuesIncorrectType(j, typeid(*pointer),
typeid(Eigen::Matrix<double, -1, 1>));
}
}
};
// Handle dynamic matrices // Handle dynamic matrices
template<int N> template <int M, int N>
struct handle<Eigen::Matrix<double, -1, N> > { struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
Eigen::Matrix<double, -1, N> operator()(Key j, Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
const gtsam::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, -1, N> >&>(*pointer).value(); } catch (std::bad_cast&) {
} catch (std::bad_cast &) { // 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, -1, N>)); }
} };
}
};
// Request for a fixed vector // Handle fixed matrices
// TODO(jing): is this piece of code really needed ??? template <int M, int N>
template<int M> struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
struct handle<Eigen::Matrix<double, M, 1> > { Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
Eigen::Matrix<double, M, 1> operator()(Key j, try {
const gtsam::Value * const pointer) { // value returns a const MatrixMN&, and the return makes a copy !!!!!
try { return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
// value returns a const VectorM&, and the return makes a copy !!!!! } catch (std::bad_cast&) {
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, 1> >&>(*pointer).value(); Matrix A;
} catch (std::bad_cast &) { try {
// Check if a dynamic vector was stored // Check if a dynamic matrix was stored
Matrix A = handle<Eigen::VectorXd>()(j, pointer); // will throw if not.... A = handle_matrix<Eigen::MatrixXd, true>()(j, pointer); // will throw if not....
// Yes: check size, and throw if not a match } catch (const ValuesIncorrectType&) {
if (A.rows() != M || A.cols() != 1) // Or a dynamic vector
throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols()); A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
else }
// This is not a copy because of RVO // Yes: check size, and throw if not a match
return A; if (A.rows() != M || A.cols() != N)
} throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
} else
}; return A; // copy but not malloc
}
}
};
// Request for a fixed matrix // Handle matrices
template<int M, int N> template <int M, int N>
struct handle<Eigen::Matrix<double, M, N> > { struct handle<Eigen::Matrix<double, M, N>> {
Eigen::Matrix<double, M, N> operator()(Key j, Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
const gtsam::Value * const pointer) { return handle_matrix<Eigen::Matrix<double, M, N>,
try { (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
// value returns a const MatrixMN&, and the return makes a copy !!!!! }
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N> >&>(*pointer).value(); };
} catch (std::bad_cast &) {
// Check if a dynamic matrix was stored
Matrix A = handle<Eigen::MatrixXd>()(j, pointer); // will throw if not....
// Yes: check size, and throw if not a match
if (A.rows() != M || A.cols() != N)
throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
else
// This is not a copy because of RVO
return A;
}
}
};
} // internal
} // internal
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ValueType> template<typename ValueType>
@ -402,47 +377,16 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// wrap all fixed in dynamics when insert and update
namespace internal {
// general type
template<typename ValueType>
struct handle_wrap {
inline gtsam::GenericValue<ValueType> operator()(Key j, const ValueType& val) {
return gtsam::GenericValue<ValueType>(val);
}
};
// when insert/update a fixed size vector: convert to dynamic size
template<int M>
struct handle_wrap<Eigen::Matrix<double, M, 1> > {
inline gtsam::GenericValue<Eigen::Matrix<double, -1, 1> > operator()(
Key j, const Eigen::Matrix<double, M, 1>& val) {
return gtsam::GenericValue<Eigen::Matrix<double, -1, 1> >(val);
}
};
// when insert/update a fixed size matrix: convert to dynamic size
template<int M, int N>
struct handle_wrap<Eigen::Matrix<double, M, N> > {
inline gtsam::GenericValue<Eigen::Matrix<double, -1, -1> > operator()(
Key j, const Eigen::Matrix<double, M, N>& val) {
return gtsam::GenericValue<Eigen::Matrix<double, -1, -1> >(val);
}
};
}
// insert a templated value // insert a templated value
template<typename ValueType> template<typename ValueType>
void Values::insert(Key j, const ValueType& val) { void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val))); insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
} }
// update with templated value // update with templated value
template <typename ValueType> template <typename ValueType>
void Values::update(Key j, const ValueType& val) { void Values::update(Key j, const ValueType& val) {
update(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val))); update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
} }
} }

20
gtsam/nonlinear/tests/testValues.cpp
View File

@ -511,16 +511,16 @@ TEST(Values, VectorFixedInsertFixedRead) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Values, VectorFixedInsertDynamicRead) { //TEST(Values, VectorFixedInsertDynamicRead) {
Values values; // Values values;
Vector3 v; v << 5.0, 6.0, 7.0; // Vector3 v; v << 5.0, 6.0, 7.0;
values.insert(key1, v); // values.insert(key1, v);
Vector expected(3); expected << 5.0, 6.0, 7.0; // Vector expected(3); expected << 5.0, 6.0, 7.0;
Vector actual = values.at<Vector>(key1); // Vector actual = values.at<Vector>(key1);
LONGS_EQUAL(3, actual.rows()); // LONGS_EQUAL(3, actual.rows());
LONGS_EQUAL(1, actual.cols()); // LONGS_EQUAL(1, actual.cols());
CHECK(assert_equal(expected, actual)); // CHECK(assert_equal(expected, actual));
} //}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Values, MatrixDynamicInsertFixedRead) { TEST(Values, MatrixDynamicInsertFixedRead) {