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

Removed all fixed hacks - replaced it with possibly something worse (no reference returned from at)

release/4.3a0
dellaert 2015-01-19 20:38:24 +01:00
parent 47811e3a64
commit c7298da4fc
5 changed files with 141 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam.h
View File

@ -1722,9 +1722,6 @@ class Values {
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
// Fixed size version
void insertFixed(size_t j, Vector t, size_t n);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
@ -1742,12 +1739,6 @@ class Values {
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
T at(size_t j);
/// Fixed size versions, for n in 1..9
void insertFixed(size_t j, Vector t, size_t n);
/// Fixed size versions, for n in 1..9
Vector atFixed(size_t j, size_t n);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;

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

@ -252,23 +252,97 @@ namespace gtsam {
return filter(key_value.key);
}
/* ************************************************************************* */
template<typename ValueType>
const ValueType& Values::at(Key j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
/* ************************************************************************* */
// Throw exception if it does not exist
if(item == values_.end())
throw ValuesKeyDoesNotExist("retrieve", j);
namespace internal {
// Check the type and throw exception if incorrect
try {
return dynamic_cast<const GenericValue<ValueType>&>(*item->second).value();
} catch (std::bad_cast &) {
throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
}
}
template<typename ValueType>
struct handle {
ValueType operator()(Key j, const gtsam::Value * const pointer) {
try {
return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value();
} catch (std::bad_cast &) {
throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
}
}
};
// Handle dynamic vectors
template<>
struct handle<Eigen::Matrix<double, -1, 1> > {
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();
} catch (std::bad_cast &) {
throw ValuesIncorrectType(j, typeid(*pointer),
typeid(Eigen::Matrix<double, -1, 1>));
}
}
};
// Handle dynamic matrices
template<int N>
struct handle<Eigen::Matrix<double, -1, N> > {
Eigen::Matrix<double, -1, N> operator()(Key j,
const gtsam::Value * const pointer) {
try {
return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, N> >&>(*pointer).value();
} catch (std::bad_cast &) {
throw ValuesIncorrectType(j, typeid(*pointer),
typeid(Eigen::Matrix<double, -1, N>));
}
}
};
// Request for a fixed vector
template<int M>
struct handle<Eigen::Matrix<double, M, 1> > {
Eigen::Matrix<double, M, 1> operator()(Key j,
const gtsam::Value * const pointer) {
try {
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, 1> >&>(*pointer).value();
} catch (std::bad_cast &) {
Matrix A = handle<Eigen::VectorXd>()(j, pointer);
if (A.rows() != M || A.cols() != 1)
throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols());
else
return A;
}
}
};
// Request for a fixed matrix
template<int M, int N>
struct handle<Eigen::Matrix<double, M, N> > {
Eigen::Matrix<double, M, N> operator()(Key j,
const gtsam::Value * const pointer) {
try {
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N> >&>(*pointer).value();
} catch (std::bad_cast &) {
Matrix A = handle<Eigen::MatrixXd>()(j, pointer);
if (A.rows() != M || A.cols() != N)
throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
else
return A;
}
}
};
} // internal
/* ************************************************************************* */
template<typename ValueType>
ValueType Values::at(Key j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
// Throw exception if it does not exist
if(item == values_.end())
throw ValuesKeyDoesNotExist("at", j);
return internal::handle<ValueType>()(j,item->second);
}
/* ************************************************************************* */
template<typename ValueType>

55
gtsam/nonlinear/Values.cpp
View File

@ -25,8 +25,6 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>
#include <list>
#include <boost/foreach.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
@ -38,6 +36,9 @@
#endif
#include <boost/iterator/transform_iterator.hpp>
#include <list>
#include <sstream>
using namespace std;
namespace gtsam {
@ -112,24 +113,6 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
Vector Values::atFixed(Key j, size_t n) {
switch (n) {
case 1: return at<Vector1>(j);
case 2: return at<Vector2>(j);
case 3: return at<Vector3>(j);
case 4: return at<Vector4>(j);
case 5: return at<Vector5>(j);
case 6: return at<Vector6>(j);
case 7: return at<Vector7>(j);
case 8: return at<Vector8>(j);
case 9: return at<Vector9>(j);
default:
throw runtime_error(
"Values::at fixed size can only handle n in 1..9");
}
}
/* ************************************************************************* */
const Value& Values::at(Key j) const {
// Find the item
@ -148,24 +131,6 @@ namespace gtsam {
throw ValuesKeyAlreadyExists(j);
}
/* ************************************************************************* */
void Values::insertFixed(Key j, const Vector& v, size_t n) {
switch (n) {
case 1: insert<Vector1>(j,v); break;
case 2: insert<Vector2>(j,v); break;
case 3: insert<Vector3>(j,v); break;
case 4: insert<Vector4>(j,v); break;
case 5: insert<Vector5>(j,v); break;
case 6: insert<Vector6>(j,v); break;
case 7: insert<Vector7>(j,v); break;
case 8: insert<Vector8>(j,v); break;
case 9: insert<Vector9>(j,v); break;
default:
throw runtime_error(
"Values::insert fixed size can only handle n in 1..9");
}
}
/* ************************************************************************* */
void Values::insert(const Values& values) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
@ -267,4 +232,18 @@ namespace gtsam {
return message_.c_str();
}
/* ************************************************************************* */
const char* NoMatchFoundForFixed::what() const throw() {
if(message_.empty()) {
ostringstream oss;
oss
<< "Attempting to retrieve fixed-size matrix with dimensions " //
<< M1_ << "x" << N1_
<< ", but found dynamic Matrix with mismatched dimensions " //
<< M2_ << "x" << N2_;
message_ = oss.str();
}
return message_.c_str();
}
}

34
gtsam/nonlinear/Values.h
View File

@ -175,14 +175,10 @@ namespace gtsam {
* @param j Retrieve the value associated with this key
* @tparam Value The type of the value stored with this key, this method
* throws DynamicValuesIncorrectType if this requested type is not correct.
* @return A const reference to the stored value
* @return The stored value
*/
template<typename ValueType>
const ValueType& at(Key j) const;
/// Special version for small fixed size vectors, for matlab/python
/// throws truntime error if n<1 || n>9
Vector atFixed(Key j, size_t n);
ValueType at(Key j) const;
/// version for double
double atDouble(size_t key) const { return at<double>(key);}
@ -264,10 +260,6 @@ namespace gtsam {
template <typename ValueType>
void insert(Key j, const ValueType& val);
/// Special version for small fixed size vectors, for matlab/python
/// throws truntime error if n<1 || n>9
void insertFixed(Key j, const Vector& v, size_t n);
/// version for double
void insertDouble(Key j, double c) { insert<double>(j,c); }
@ -505,6 +497,28 @@ namespace gtsam {
}
};
/* ************************************************************************* */
class GTSAM_EXPORT NoMatchFoundForFixed: public std::exception {
protected:
const size_t M1_, N1_;
const size_t M2_, N2_;
private:
mutable std::string message_;
public:
NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () :
M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
}
virtual ~NoMatchFoundForFixed() throw () {
}
virtual const char* what() const throw ();
};
/* ************************************************************************* */
/// traits
template<>
struct traits<Values> : public Testable<Values> {

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

@ -476,13 +476,22 @@ TEST(Values, Destructors) {
}
/* ************************************************************************* */
TEST(Values, FixedSize) {
TEST(Values, FixedVector) {
Values values;
Vector v(3); v << 5.0, 6.0, 7.0;
values.insertFixed(key1, v, 3);
values.insert(key1, v);
Vector3 expected(5.0, 6.0, 7.0);
CHECK(assert_equal((Vector)expected, values.at<Vector3>(key1)));
CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error);
CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
}
/* ************************************************************************* */
TEST(Values, FixedMatrix) {
Values values;
Matrix v(1,3); v << 5.0, 6.0, 7.0;
values.insert(key1, v);
Vector3 expected(5.0, 6.0, 7.0);
CHECK(assert_equal((Vector)expected, values.at<Matrix13>(key1)));
CHECK_EXCEPTION(values.at<Matrix23>(key1), exception);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }