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

unwhitened error in LinearApproxFactor now works

release/4.3a0
Alex Cunningham 2010-10-11 04:47:57 +00:00
parent 82e6c63d13
commit ccea5c79cb
2 changed files with 20 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
slam/LinearApproxFactor-inl.h
View File

@ -42,14 +42,16 @@ LinearApproxFactor<Values,Key>::LinearApproxFactor(
template <class Values, class Key> template <class Values, class Key>
Vector LinearApproxFactor<Values,Key>::unwhitenedError(const Values& c) const { Vector LinearApproxFactor<Values,Key>::unwhitenedError(const Values& c) const {
// extract the points in the new config // extract the points in the new config
VectorValues delta; Vector ret = - b_;
// BOOST_FOREACH(const Key& key, nonlinearKeys_) {
// X newPt = c[key], linPt = lin_points_[key];
// Vector d = linPt.logmap(newPt);
// delta.insert(key, d);
// }
return zero(b_.size()); //FIXME: PLACEHOLDER! BOOST_FOREACH(const Key& key, nonlinearKeys_) {
X newPt = c[key], linPt = lin_points_[key];
Vector d = linPt.logmap(newPt);
const Matrix& A = matrices_.at(Symbol(key));
ret += prod(A, d);
}
return ret;
} }
/* ************************************************************************* */ /* ************************************************************************* */

16
tests/testLinearApproxFactor.cpp
View File

@ -1,4 +1,4 @@
/* /**
* @file testLinearApproxFactor.cpp * @file testLinearApproxFactor.cpp
* @brief tests for dummy factor that contains a linear factor * @brief tests for dummy factor that contains a linear factor
* @author Alex Cunningham * @author Alex Cunningham
@ -26,17 +26,23 @@ TEST ( LinearApproxFactor, basic ) {
Ordering ordering; Ordering ordering;
ordering.push_back(key1); ordering.push_back(key1);
planarSLAM::Values lin_points; planarSLAM::Values lin_points;
planarSLAM::PointKey PKey(1);
Point2 point(1.0, 2.0);
lin_points.insert(PKey, point);
ApproxFactor f1(lin_factor, ordering, lin_points); ApproxFactor f1(lin_factor, ordering, lin_points);
EXPECT(f1.size() == 1); EXPECT(f1.size() == 1);
ApproxFactor::KeyVector expKeyVec; EXPECT(assert_equal(key1, f1.keys().front()));
expKeyVec.push_back(planarSLAM::PointKey(key1.index())); EXPECT(assert_equal(b, f1.get_b()));
planarSLAM::Values config; // doesn't really need to have any data planarSLAM::Values config;
config.insert(PKey, Point2(2.0, 3.0));
GaussianFactor::shared_ptr actual = f1.linearize(config, ordering); GaussianFactor::shared_ptr actual = f1.linearize(config, ordering);
EXPECT(assert_equal(Vector_(2, -0.2, -0.2), f1.unwhitenedError(config)));
// Check the linearization // Check the linearization
CHECK(assert_equal(*lin_factor, *actual)); EXPECT(assert_equal(*lin_factor, *actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */