diff --git a/gtsam_unstable/base/tests/testBAD.cpp b/gtsam_unstable/base/tests/testBAD.cpp index 2414b7a76..878dcb77e 100644 --- a/gtsam_unstable/base/tests/testBAD.cpp +++ b/gtsam_unstable/base/tests/testBAD.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include @@ -130,27 +129,6 @@ public: }; //----------------------------------------------------------------------------- - -Point3 transformTo(const Pose3& x, const Point3& p) { - return x.transform_to(p); -} - -Point2 project(const Point3& p) { - return PinholeCamera::project_to_camera(p); -} - -/// Expression version of uncalibrate -template -LeafExpression uncalibrate(const E1& K, const E2& p) { - return LeafExpression(0); -} - -/// Expression version of Point2.sub -template -LeafExpression operator -(const E1& p, const E2& q) { - return LeafExpression(0); -} - /// AD Factor template class BADFactor: NonlinearFactor { @@ -180,7 +158,7 @@ public: virtual double error(const Values& values) const { if (this->active(values)) { const Vector e = unwhitenedError(values); - return 0.5 * e.norm(); + return 0.5 * e.squaredNorm(); } else { return 0.0; } @@ -209,6 +187,21 @@ public: using namespace std; using namespace gtsam; +//----------------------------------------------------------------------------- + +Point3 transformTo(const Pose3& x, const Point3& p) { + return x.transform_to(p); +} + +Point2 project(const Point3& p) { + return PinholeCamera::project_to_camera(p); +} + +template +Point2 uncalibrate(const CAL& K, const Point2& p) { + return K.uncalibrate(p); +} + /* ************************************************************************* */ TEST(BAD, test) { @@ -220,7 +213,7 @@ TEST(BAD, test) { values.insert(3, Cal3_S2()); // Create old-style factor to create expected value and derivatives - Point2 measured(0, 1); + Point2 measured(-17, 30); SharedNoiseModel model = noiseModel::Unit::Create(2); GeneralSFMFactor2 old(measured, model, 1, 2, 3); double expected_error = old.error(values); @@ -237,10 +230,12 @@ TEST(BAD, test) { typedef UnaryExpression Unary1; Unary1 projection(project, p_cam); - LeafExpression uv_hat = uncalibrate(K, projection); + + typedef BinaryExpression, Unary1> Binary2; + Binary2 uv_hat(uncalibrate, K, projection); // Create factor - BADFactor > f(measured, uv_hat); + BADFactor f(measured, uv_hat); // Check value EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);