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

keys now functional

release/4.3a0
dellaert 2014-09-27 16:08:59 +02:00
parent 186afcc95e
commit ab1f4c1e32
1 changed files with 38 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam_unstable/base/tests/testBAD.cpp
View File

@ -44,7 +44,11 @@ protected:
public: public:
virtual ~ExpressionNode() { virtual ~ExpressionNode() {
} }
virtual void getKeys(std::set<Key>& keys) const = 0;
/// Return keys that play in this expression as a set
virtual std::set<Key> keys() const = 0;
/// Return value and optional derivatives
virtual T value(const Values& values, virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> = boost::none) const = 0; boost::optional<std::map<Key, Matrix>&> = boost::none) const = 0;
}; };
@ -70,8 +74,13 @@ public:
virtual ~ConstantExpression() { virtual ~ConstantExpression() {
} }
virtual void getKeys(std::set<Key>& /* keys */) const { /// Return keys that play in this expression, i.e., the empty set
virtual std::set<Key> keys() const {
std::set<Key> keys;
return keys;
} }
/// Return value and optional derivatives
virtual T value(const Values& values, virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
return value_; return value_;
@ -97,9 +106,14 @@ public:
virtual ~LeafExpression() { virtual ~LeafExpression() {
} }
virtual void getKeys(std::set<Key>& keys) const { /// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys;
keys.insert(key_); keys.insert(key_);
return keys;
} }
/// Return value and optional derivatives
virtual T value(const Values& values, virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
const T& value = values.at<T>(key_); const T& value = values.at<T>(key_);
@ -143,9 +157,12 @@ public:
virtual ~UnaryExpression() { virtual ~UnaryExpression() {
} }
virtual void getKeys(std::set<Key>& keys) const { /// Return keys that play in this expression
expression_->getKeys(keys); virtual std::set<Key> keys() const {
return expression_->keys();
} }
/// Return value and optional derivatives
virtual T value(const Values& values, virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
@ -195,10 +212,15 @@ public:
virtual ~BinaryExpression() { virtual ~BinaryExpression() {
} }
virtual void getKeys(std::set<Key>& keys) const { /// Return keys that play in this expression
expression1_->getKeys(keys); virtual std::set<Key> keys() const {
expression2_->getKeys(keys); std::set<Key> keys1 = expression1_->keys();
std::set<Key> keys2 = expression2_->keys();
keys1.insert(keys2.begin(), keys2.end());
return keys1;
} }
/// Return value and optional derivatives
virtual T value(const Values& values, virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
T val; T val;
@ -266,8 +288,8 @@ public:
root_.reset(new BinaryExpression<T, E1, E2>(f, expression1, expression2)); root_.reset(new BinaryExpression<T, E1, E2>(f, expression1, expression2));
} }
void getKeys(std::set<Key>& keys) const { std::set<Key> keys() const {
root_->getKeys(keys); return root_->keys();
} }
T value(const Values& values, T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
@ -397,10 +419,12 @@ TEST(BAD, test) {
Expression<Point2> projection(project, p_cam); Expression<Point2> projection(project, p_cam);
Expression<Point2> uv_hat(uncalibrate, K, projection); Expression<Point2> uv_hat(uncalibrate, K, projection);
// Check getKeys // Check keys
std::set<Key> keys; std::set<Key> expectedKeys;
uv_hat.getKeys(keys); expectedKeys.insert(1);
EXPECT_LONGS_EQUAL(3, keys.size()); expectedKeys.insert(2);
expectedKeys.insert(3);
EXPECT(expectedKeys == uv_hat.keys());
// Create factor // Create factor
BADFactor<Point2> f(measured, uv_hat); BADFactor<Point2> f(measured, uv_hat);