diff --git a/gtsam_unstable/nonlinear/BADFactor.h b/gtsam_unstable/nonlinear/BADFactor.h index 6780afd73..0bc27663c 100644 --- a/gtsam_unstable/nonlinear/BADFactor.h +++ b/gtsam_unstable/nonlinear/BADFactor.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -26,58 +27,46 @@ namespace gtsam { * BAD Factor that supports arbitrary expressions via AD */ template -class BADFactor: NonlinearFactor { +class BADFactor: public NoiseModelFactor { const T measurement_; const Expression expression_; - /// get value from expression and calculate error with respect to measurement - Vector unwhitenedError(const Values& values) const { - const T& value = expression_.value(values); - return value.localCoordinates(measurement_); - } - public: /// Constructor - BADFactor(const T& measurement, const Expression& expression) : + BADFactor(const SharedNoiseModel& noiseModel, // + const T& measurement, const Expression& expression) : + NoiseModelFactor(noiseModel, expression.keys()), // measurement_(measurement), expression_(expression) { } - /// Constructor - BADFactor(const T& measurement, const ExpressionNode& expression) : - measurement_(measurement), expression_(expression) { - } - /** - * Calculate the error of the factor. - * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. - * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model - * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. - */ - virtual double error(const Values& values) const { - if (this->active(values)) { - const Vector e = unwhitenedError(values); - return 0.5 * e.squaredNorm(); - } else { - return 0.0; - } - } /// get the dimension of the factor (number of rows on linearization) size_t dim() const { return measurement_.dim(); } - /// linearize to a GaussianFactor - boost::shared_ptr linearize(const Values& values) const { - // We will construct an n-ary factor below, where terms is a container whose - // value type is std::pair, specifying the - // collection of keys and matrices making up the factor. - std::map terms; - expression_.value(values, terms); - Vector b = unwhitenedError(values); - SharedDiagonal model = SharedDiagonal(); - return boost::shared_ptr( - new JacobianFactor(terms, b, model)); + /** + * Error function *without* the NoiseModel, \f$ z-h(x) \f$. + * We override this method to provide + * both the function evaluation and its derivative(s) in H. + */ + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + if (H) { + typedef std::map MapType; + MapType terms; + const T& value = expression_.value(x, terms); + // copy terms to H + H->clear(); + H->reserve(terms.size()); + for (MapType::iterator it = terms.begin(); it != terms.end(); ++it) + H->push_back(it->second); + return measurement_.localCoordinates(value); + } else { + const T& value = expression_.value(x); + return measurement_.localCoordinates(value); + } } }; diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp index 6cadc4f63..7ffb4530d 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -59,15 +59,21 @@ TEST(BADFactor, test) { Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); - // Create factor and check value, dimension, linearization - BADFactor f(measured, uv_hat); + // Create factor and check unwhitenedError + BADFactor f(model, measured, uv_hat); + std::vector 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 gf = f.linearize(values); EXPECT( assert_equal(*expected, *gf, 1e-9)); // Try concise version - BADFactor f2(measured, uncalibrate(K, project(transform_to(x, p)))); + BADFactor f2(model, measured, + uncalibrate(K, project(transform_to(x, p)))); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); boost::shared_ptr gf2 = f2.linearize(values); @@ -83,13 +89,20 @@ TEST(BADFactor, compose) { Rot3_ R3 = R1 * R2; // Create factor - BADFactor f(Rot3(), R3); + BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); // Create some values Values values; values.insert(1, Rot3()); values.insert(2, Rot3()); + // Check unwhitenedError + std::vector H; + 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)); + // Check linearization JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); boost::shared_ptr gf = f.linearize(values); @@ -107,12 +120,18 @@ TEST(BADFactor, compose2) { Rot3_ R3 = R1 * R2; // Create factor - BADFactor f(Rot3(), R3); + BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); // Create some values Values values; values.insert(1, Rot3()); + // Check unwhitenedError + std::vector H; + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(2*eye(3), H[0],1e-9)); + // Check linearization JacobianFactor expected(1, 2 * eye(3), zero(3)); boost::shared_ptr gf = f.linearize(values);