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

Revert "FIx indentation for Values-inl.h" 

This reverts commit 5749565e52.
release/4.3a0
Varun Agrawal 2020-11-30 10:43:58 -05:00
parent 5749565e52
commit f8eece464d
1 changed files with 80 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -259,98 +259,97 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<> template<>
inline bool Values::filterHelper<Value>(const boost::function<bool(Key)> filter, inline bool Values::filterHelper<Value>(const boost::function<bool(Key)> filter,
const ConstKeyValuePair& key_value) { const ConstKeyValuePair& key_value) {
// Filter and check the type // Filter and check the type
return filter(key_value.key); return filter(key_value.key);
} }
/* ************************************************************************* */ /* ************************************************************************* */
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 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));
} }
} }
}; };
template <typename MatrixType, bool isDynamic> template <typename MatrixType, bool isDynamic>
struct handle_matrix; struct handle_matrix;
// 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) { 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();
} 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, M, N>));
} }
} }
}; };
// 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) { Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
try { try {
// 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&) { } catch (std::bad_cast&) {
Matrix A; Matrix A;
try { 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.... A = handle_matrix<Eigen::MatrixXd, true>()(j, pointer); // will throw if not....
} catch (const ValuesIncorrectType&) { } catch (const ValuesIncorrectType&) {
// 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....
} }
// Yes: check size, and throw if not a match // Yes: check size, and throw if not a match
if (A.rows() != M || A.cols() != N) if (A.rows() != M || A.cols() != N)
throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
else else
return A; // copy but not malloc return A; // copy but not malloc
} }
} }
}; };
// Handle matrices // 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, const Value* const pointer) { Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
return handle_matrix<Eigen::Matrix<double, M, N>, return handle_matrix<Eigen::Matrix<double, M, N>,
(M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer); (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
} }
}; };
} // internal } // internal
/* ************************************************************************* /* ************************************************************************* */
*/ template <typename ValueType>
template <typename ValueType> const ValueType Values::at(Key j) const {
const ValueType Values::at(Key j) const { // Find the item
// Find the item KeyValueMap::const_iterator item = values_.find(j);
KeyValueMap::const_iterator item = values_.find(j);
// Throw exception if it does not exist // Throw exception if it does not exist
if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j); if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
// Check the type and throw exception if incorrect // Check the type and throw exception if incorrect
// h() split in two lines to avoid internal compiler error (MSVC2017) // h() split in two lines to avoid internal compiler error (MSVC2017)
auto h = internal::handle<ValueType>(); auto h = internal::handle<ValueType>();
return h(j, item->second); return h(j, item->second);
} }
/* ************************************************************************* */ /* ************************************************************************* */