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

gtsam/nonlinear/NonlinearFactor.cpp

@@ -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);

gtsam_unstable/nonlinear/BADFactor.h

@@ -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
+      // move terms to H, which is pre-allocated to correct size
-      H->clear();
+      size_t j = 0;
-      H->reserve(terms.size());
       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);

gtsam_unstable/nonlinear/tests/testBADFactor.cpp

@@ -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));