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

try to fix FixedValues branch by wrap all Eigen type in Dynamic size in Values, since fixed size Eigen type cannot read out by GenericValue if size is not explictly given

release/4.3a0
Jing Dong 2015-04-05 18:24:17 -04:00
parent 9755b95e89
commit e1faf8874e
2 changed files with 56 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -274,7 +274,7 @@ namespace gtsam {
Eigen::Matrix<double, -1, 1> operator()(Key j,
const gtsam::Value * const pointer) {
try {
return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
return reinterpret_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
} catch (std::bad_cast &) {
throw ValuesIncorrectType(j, typeid(*pointer),
typeid(Eigen::Matrix<double, -1, 1>));
@ -367,16 +367,48 @@ 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
template<typename ValueType>
void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
}
void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
}
// update with templated value
template <typename ValueType>
void Values::update(Key j, const ValueType& val) {
update(j, static_cast<const Value&>(GenericValue<ValueType >(val)));
update(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
}
}

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

@ -487,14 +487,15 @@ TEST(Values, VectorDynamicInsertFixedRead) {
}
/* ************************************************************************* */
TEST(Values, VectorFixedInsertDynamicRead) {
TEST(Values, VectorDynamicInsertDynamicRead) {
Values values;
Vector3 v; v << 5.0, 6.0, 7.0;
Vector v(3); v << 5.0, 6.0, 7.0;
values.insert(key1, v);
Vector expected(3); expected << 5.0, 6.0, 7.0;
Vector actual(3); actual = values.at<Vector>(key1);
//CHECK(assert_equal(expected, actual));
CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
Vector actual = values.at<Vector>(key1);
LONGS_EQUAL(3, actual.rows());
LONGS_EQUAL(1, actual.cols());
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
@ -504,10 +505,22 @@ TEST(Values, VectorFixedInsertFixedRead) {
values.insert(key1, v);
Vector3 expected; expected << 5.0, 6.0, 7.0;
Vector3 actual = values.at<Vector3>(key1);
//CHECK(assert_equal(expected, actual));
CHECK(assert_equal(expected, actual));
CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
}
/* ************************************************************************* */
TEST(Values, VectorFixedInsertDynamicRead) {
Values values;
Vector3 v; v << 5.0, 6.0, 7.0;
values.insert(key1, v);
Vector expected(3); expected << 5.0, 6.0, 7.0;
Vector actual = values.at<Vector>(key1);
LONGS_EQUAL(3, actual.rows());
LONGS_EQUAL(1, actual.cols());
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Values, MatrixDynamicInsertFixedRead) {
Values values;