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

Assume H pre-allocated as usual, and *move* Jacobians to avoid allocations

release/4.3a0
dellaert 2014-10-02 13:30:16 +02:00
parent 0800b83285
commit df17758469
3 changed files with 13 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -72,14 +72,6 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
&& noiseModel_->equals(*e->noiseModel_, tol)));
}
Vector NoiseModelFactor::whitenedError(const Values& c) const {
const Vector b = unwhitenedError(c);
if ((size_t) b.size() != noiseModel_->dim())
throw std::invalid_argument(
"This factor was created with a NoiseModel of incorrect dimension.");
return noiseModel_->whiten(b);
}
static void check(const SharedNoiseModel& noiseModel, const Vector& b) {
if (!noiseModel)
throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
@ -88,6 +80,12 @@ static void check(const SharedNoiseModel& noiseModel, const Vector& b) {
"NoiseModelFactor was created with a NoiseModel of incorrect dimension.");
}
Vector NoiseModelFactor::whitenedError(const Values& c) const {
const Vector b = unwhitenedError(c);
check(noiseModel_, b);
return noiseModel_->whiten(b);
}
double NoiseModelFactor::error(const Values& c) const {
if (this->active(c)) {
const Vector b = unwhitenedError(c);

13
gtsam_unstable/nonlinear/BADFactor.h
View File

@ -41,11 +41,6 @@ public:
measurement_(measurement), expression_(expression) {
}
/// get the dimension of the factor (number of rows on linearization)
size_t dim() const {
return measurement_.dim();
}
/**
* Error function *without* the NoiseModel, \f$ z-h(x) \f$.
* We override this method to provide
@ -54,14 +49,14 @@ public:
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
if (H) {
assert(H->size()==size());
typedef std::map<Key, Matrix> MapType;
MapType terms;
const T& value = expression_.value(x, terms);
// copy terms to H
H->clear();
H->reserve(terms.size());
// move terms to H, which is pre-allocated to correct size
size_t j = 0;
for (MapType::iterator it = terms.begin(); it != terms.end(); ++it)
H->push_back(it->second);
it->second.swap((*H)[j++]);
return measurement_.localCoordinates(value);
} else {
const T& value = expression_.value(x);

12
gtsam_unstable/nonlinear/tests/testBADFactor.cpp
View File

@ -59,13 +59,8 @@ TEST(BADFactor, test) {
Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
// Create factor and check unwhitenedError
// Create factor and check value, dimension, linearization
BADFactor<Point2> f(model, measured, uv_hat);
std::vector<Matrix> H;
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(3, H.size());
// Check value, dimension, linearization
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
@ -97,9 +92,8 @@ TEST(BADFactor, compose) {
values.insert(2, Rot3());
// Check unwhitenedError
std::vector<Matrix> H;
std::vector<Matrix> H(2);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(2, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9));
@ -127,7 +121,7 @@ TEST(BADFactor, compose2) {
values.insert(1, Rot3());
// Check unwhitenedError
std::vector<Matrix> H;
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(2*eye(3), H[0],1e-9));