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

Merge remote-tracking branch 'origin/feature/FixFixedValues' into feature/Feature/FixedValues 

Conflicts:
	gtsam/nonlinear/Values-inl.h
release/4.3a0
Duy-Nguyen Ta 2015-10-24 10:08:53 -04:00
parent d4021859f7 e1faf8874e
commit 5f396856f3
2 changed files with 59 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -275,8 +275,12 @@ namespace gtsam {
Eigen::Matrix<double, -1, 1> operator()(Key j,
const gtsam::Value * const pointer) {
try {
//<<<<<<< HEAD
// value returns a const Vector&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
//=======
// return reinterpret_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
//>>>>>>> feature/FixFixedValues
} catch (std::bad_cast &) {
// If a fixed vector was stored, we end up here as well.
throw ValuesIncorrectType(j, typeid(*pointer),
@ -379,16 +383,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

@ -488,14 +488,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));
}
/* ************************************************************************* */
@ -505,10 +506,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;