diff --git a/.cproject b/.cproject index 7f15e2ad7..05e61f875 100644 --- a/.cproject +++ b/.cproject @@ -1,4 +1,4 @@ - + @@ -298,6 +298,22 @@ + +make +-j2 +install +true +true +true + + +make +-j2 +check +true +true +true + make -k @@ -452,7 +468,7 @@ make - + testBayesTree.run true false @@ -467,7 +483,7 @@ make - + testSymbolicFactorGraph.run true false @@ -723,7 +739,7 @@ make - + testGraph.run true false @@ -737,10 +753,10 @@ true true - + make -j2 -testTupleConfig.run +testRangeFactor.run true true true @@ -769,22 +785,6 @@ true true - -make --j2 -install -true -true -true - - -make --j2 -check -true -true -true - diff --git a/cpp/Makefile.am b/cpp/Makefile.am index f9660c98b..d2013f47e 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -214,11 +214,13 @@ testPose2Graph_SOURCES = $(example) testPose2Graph.cpp testPose2Graph_LDADD = libgtsam.la # 2D Bearing and Range -headers += BearingFactor.h +headers += BearingFactor.h RangeFactor.h sources += -check_PROGRAMS += testBearingFactor +check_PROGRAMS += testBearingFactor testRangeFactor testBearingFactor_SOURCES = $(example) testBearingFactor.cpp testBearingFactor_LDADD = libgtsam.la +testRangeFactor_SOURCES = $(example) testRangeFactor.cpp +testRangeFactor_LDADD = libgtsam.la # 3D Pose constraints headers += Pose3Factor.h diff --git a/cpp/RangeFactor.h b/cpp/RangeFactor.h new file mode 100644 index 000000000..7c6f6503b --- /dev/null +++ b/cpp/RangeFactor.h @@ -0,0 +1,68 @@ +/** + * @file RangeFactor.H + * @authors Frank Dellaert + **/ + +#pragma once + +#include "Pose2.h" +#include "Point2.h" +#include "NonlinearFactor.h" + +namespace gtsam { + + /** + * Calculate range to a landmark + * @param pose 2D pose of robot + * @param point 2D location of landmark + * @return range (double) + */ + double range(const Pose2& pose, const Point2& point) { + Point2 d = transform_to(pose, point); + return d.norm(); + } + + /** + * Calculate range and optional derivative(s) + */ + double range(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2) { + if (!H1 && !H2) return range(pose, point); + Point2 d = transform_to(pose, point); + double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); + Matrix D_result_d = Matrix_(1, 2, x / n, y / n); + if (H1) *H1 = D_result_d * Dtransform_to1(pose, point); + if (H2) *H2 = D_result_d * Dtransform_to2(pose, point); + return n; + } + + /** + * Non-linear factor for a constraint derived from a 2D measurement, + * i.e. the main building block for visual SLAM. + */ + template + class RangeFactor: public NonlinearFactor2 { + private: + + double z_; /** measurement */ + + typedef NonlinearFactor2 Base; + + public: + + RangeFactor(); /* Default constructor */ + + RangeFactor(double z, double sigma, const PoseKey& i, const PointKey& j) : + Base(sigma, i, j), z_(z) { + } + + /** h(x)-z */ + Vector evaluateError(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2) const { + double hx = gtsam::range(pose, point, H1, H2); + return Vector_(1, hx - z_); + } + }; // RangeFactor + +} // namespace gtsam diff --git a/cpp/testRangeFactor.cpp b/cpp/testRangeFactor.cpp new file mode 100644 index 000000000..5836842a5 --- /dev/null +++ b/cpp/testRangeFactor.cpp @@ -0,0 +1,81 @@ +/** + * @file testRangeFactor.cpp + * @brief Unit tests for RangeFactor Class + * @authors Frank Dellaert + **/ + +#include + +#include "numericalDerivative.h" +#include "RangeFactor.h" +#include "TupleConfig.h" + +using namespace std; +using namespace gtsam; + +// typedefs +typedef Symbol PoseKey; +typedef Symbol PointKey; +typedef PairConfig Config; +typedef RangeFactor MyFactor; + +// some shared test values +Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); +Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); + +/* ************************************************************************* */ +TEST( RangeFactor, range ) +{ + Matrix expectedH1, actualH1, expectedH2, actualH2; + + // establish range is indeed zero + DOUBLES_EQUAL(1,gtsam::range(x1,l1),1e-9); + + // establish range is indeed 45 degrees + DOUBLES_EQUAL(sqrt(2),gtsam::range(x1,l2),1e-9); + + // Another pair + double actual23 = gtsam::range(x2, l3, actualH1, actualH2); + DOUBLES_EQUAL(sqrt(2),actual23,1e-9); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(range, x2, l3, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + expectedH2 = numericalDerivative22(range, x2, l3, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + + // Another test + double actual34 = gtsam::range(x3, l4, actualH1, actualH2); + DOUBLES_EQUAL(2,actual34,1e-9); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(range, x3, l4, 1e-5); + expectedH2 = numericalDerivative22(range, x3, l4, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + CHECK(assert_equal(expectedH1,actualH1)); +} + +/* ************************************************************************* */ +TEST( RangeFactor, evaluateError ) +{ + // Create factor + double z(sqrt(2) - 0.22); // h(x) - z = 0.22 + double sigma = 0.1; + MyFactor factor(z, sigma, 2, 3); + + // create config + Config c; + c.insert(2, x2); + c.insert(3, l3); + + // Check error + Vector actual = factor.error_vector(c); + CHECK(assert_equal(Vector_(1,0.22),actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */