diff --git a/cpp/BearingFactor.h b/cpp/BearingFactor.h index 85478baea..50f0df1df 100644 --- a/cpp/BearingFactor.h +++ b/cpp/BearingFactor.h @@ -12,26 +12,6 @@ namespace gtsam { - /** - * Calculate relative bearing to a landmark in local coordinate frame - * @param point 2D location of landmark - * @param H optional reference for Jacobian - * @return 2D rotation \in SO(2) - */ - Rot2 relativeBearing(const Point2& d) { - double n = d.norm(); - return Rot2(d.x() / n, d.y() / n); - } - - /** - * Calculate relative bearing and optional derivative - */ - Rot2 relativeBearing(const Point2& d, boost::optional H) { - double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); - if (H) *H = Matrix_(1, 2, -y / d2, x / d2); - return Rot2(x / n, y / n); - } - /** * Calculate bearing to a landmark * @param pose 2D pose of robot diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index 856909f68..bead4aa5f 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -46,18 +46,18 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix Dbetween1(const Pose2& p0, const Pose2& p2) { - Matrix dt_dr = Dunrotate1(p0.r(), p2.t()-p0.t()); - Matrix dt_dt1 = -invcompose(p2.r(), p0.r()).matrix(); - Matrix dt_dr1 = Dunrotate1(p2.r(), p2.t()-p0.t()); + Matrix Dbetween1(const Pose2& p1, const Pose2& p2) { + Point2 dt = p2.t()-p1.t(); + Matrix dT1 = -invcompose(p2.r(), p1.r()).matrix(); + Matrix dR1; + unrotate(p2.r(), dt, dR1); return Matrix_(3,3, - dt_dt1(0,0), dt_dt1(0,1), dt_dr1(0,0), - dt_dt1(1,0), dt_dt1(1,1), dt_dr1(1,0), - 0.0, 0.0, -1.0); + dT1(0,0), dT1(0,1), dR1(0,0), + dT1(1,0), dT1(1,1), dR1(1,0), + 0.0, 0.0, -1.0); } - Matrix Dbetween2(const Pose2& p0, const Pose2& p2) { - Matrix db_dt2 = p0.r().transpose(); + Matrix Dbetween2(const Pose2& p1, const Pose2& p2) { return Matrix_(3,3, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp index a9dc8431f..bd216796c 100644 --- a/cpp/Rot2.cpp +++ b/cpp/Rot2.cpp @@ -12,77 +12,77 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - template class Lie; + /** Explicit instantiation of base class to export members */ + template class Lie ; - /* ************************************************************************* */ - void Rot2::print(const string& s) const { - cout << s << ":" << theta() << endl; - } + /* ************************************************************************* */ + void Rot2::print(const string& s) const { + cout << s << ":" << theta() << endl; + } - /* ************************************************************************* */ - bool Rot2::equals(const Rot2& R, double tol) const { - return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol; - } + /* ************************************************************************* */ + bool Rot2::equals(const Rot2& R, double tol) const { + return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol; + } - /* ************************************************************************* */ - Matrix Rot2::matrix() const { - return Matrix_(2, 2, c_, -s_, s_, c_); - } + /* ************************************************************************* */ + Matrix Rot2::matrix() const { + return Matrix_(2, 2, c_, -s_, s_, c_); + } - /* ************************************************************************* */ - Matrix Rot2::transpose() const { - return Matrix_(2, 2, c_, s_, -s_, c_); - } + /* ************************************************************************* */ + Matrix Rot2::transpose() const { + return Matrix_(2, 2, c_, s_, -s_, c_); + } - /* ************************************************************************* */ - Matrix Rot2::negtranspose() const { - return Matrix_(2, 2, -c_, -s_, s_, -c_); - } + /* ************************************************************************* */ + Matrix Rot2::negtranspose() const { + return Matrix_(2, 2, -c_, -s_, s_, -c_); + } - /* ************************************************************************* */ - Point2 Rot2::unrotate(const Point2& p) const { - return Point2( - c_ * p.x() + s_ * p.y(), - -s_ * p.x() + c_ * p.y() - ); - } + /* ************************************************************************* */ + Point2 Rot2::rotate(const Point2& p) const { + return Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); + } - /* ************************************************************************* */ - Point2 rotate(const Rot2& R, const Point2& p) { - return Point2( - R.c() * p.x() - R.s() * p.y(), - R.s() * p.x() + R.c() * p.y()); - } + /* ************************************************************************* */ + Point2 Rot2::unrotate(const Point2& p) const { + return Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); + } - /* ************************************************************************* */ - // see libraries/caml/geometry/math.ml - Matrix Drotate1(const Rot2& R, const Point2& p) { - Point2 q = R*p; - return Matrix_(2, 1, -q.y(), q.x()); - } + /* ************************************************************************* */ + // see libraries/caml/geometry/math.ml + Point2 rotate(const Rot2 & R, const Point2& p, + boost::optional H1, boost::optional H2) { + Point2 q = R * p; + if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x()); + if (H2) *H2 = R.matrix(); + return q; + } - /* ************************************************************************* */ - Matrix Drotate2(const Rot2& R) { - return R.matrix(); - } + /* ************************************************************************* */ + // see libraries/caml/geometry/math.lyx, derivative of unrotate + Point2 unrotate(const Rot2 & R, const Point2& p, + boost::optional H1, boost::optional H2) { + Point2 q = R.unrotate(p); + if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x()); + if (H2) *H2 = R.transpose(); + return q; + } - /* ************************************************************************* */ - Point2 unrotate(const Rot2& R, const Point2& p) { - return R.unrotate(p); - } + /* ************************************************************************* */ + Rot2 relativeBearing(const Point2& d) { + double n = d.norm(); + return Rot2(d.x() / n, d.y() / n); + } - /* ************************************************************************* */ - /** see libraries/caml/geometry/math.lyx, derivative of unrotate */ - /* ************************************************************************* */ - Matrix Dunrotate1(const Rot2 & R, const Point2 & p) { - Point2 q = R.unrotate(p); - return Matrix_(2, 1, q.y(), -q.x()); - } + /* ************************************************************************* */ + Rot2 relativeBearing(const Point2& d, boost::optional H) { + double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); + if (H) *H = Matrix_(1, 2, -y / d2, x / d2); + return Rot2(x / n, y / n); + } - /* ************************************************************************* */ - Matrix Dunrotate2(const Rot2 & R) { - return R.transpose(); - } +/* ************************************************************************* */ } // gtsam diff --git a/cpp/Rot2.h b/cpp/Rot2.h index 603e2bfdc..ab9b44a66 100644 --- a/cpp/Rot2.h +++ b/cpp/Rot2.h @@ -8,6 +8,7 @@ #ifndef ROT2_H_ #define ROT2_H_ +#include #include "Testable.h" #include "Point2.h" #include "Matrix.h" @@ -58,12 +59,12 @@ namespace gtsam { /** return 2*2 negative transpose */ Matrix negtranspose() const; + /** rotate from world to rotated = R*p */ + Point2 rotate(const Point2& p) const; + /** rotate from world to rotated = R'*p */ Point2 unrotate(const Point2& p) const; - /** friends */ - friend Matrix Dunrotate1(const Rot2& R, const Point2& p); - private: /** Serialization function */ friend class boost::serialization::access; @@ -122,20 +123,29 @@ namespace gtsam { * rotate point from rotated coordinate frame to * world = R*p */ - Point2 rotate(const Rot2& R, const Point2& p); - inline Point2 operator*(const Rot2& R, const Point2& p) { - return rotate(R,p); - } - Matrix Drotate1(const Rot2& R, const Point2& p); - Matrix Drotate2(const Rot2& R); // does not depend on p ! + inline Point2 operator*(const Rot2& R, const Point2& p) {return R.rotate(p);} + Point2 rotate(const Rot2 & R, const Point2& p, boost::optional H1 = + boost::none, boost::optional H2 = boost::none); /** * rotate point from world to rotated * frame = R'*p */ - Point2 unrotate(const Rot2& R, const Point2& p); - Matrix Dunrotate1(const Rot2& R, const Point2& p); - Matrix Dunrotate2(const Rot2& R); // does not depend on p ! + Point2 unrotate(const Rot2 & R, const Point2& p, boost::optional H1 = + boost::none, boost::optional H2 = boost::none); + + /** + * Calculate relative bearing to a landmark in local coordinate frame + * @param point 2D location of landmark + * @param H optional reference for Jacobian + * @return 2D rotation \in SO(2) + */ + Rot2 relativeBearing(const Point2& d); + + /** + * Calculate relative bearing and optional derivative + */ + Rot2 relativeBearing(const Point2& d, boost::optional H); } // gtsam diff --git a/cpp/testBearingRange.cpp b/cpp/testBearingRange.cpp new file mode 100644 index 000000000..0939d8739 --- /dev/null +++ b/cpp/testBearingRange.cpp @@ -0,0 +1,62 @@ +/** + * @file testBearingRange.cpp + * @authors Frank Dellaert + **/ + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// typedefs +typedef Symbol PoseKey; +typedef Symbol PointKey; +typedef PairConfig Config; +typedef BearingFactor BearingMeasurement; +typedef RangeFactor RangeMeasurement; +typedef NonlinearFactorGraph Graph; + +// 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( BearingRange, constructor ) +{ + // create config + Config c; + c.insert(2, x2); + c.insert(3, l3); + + // create graph + Graph G; + + // Create bearing factor + Rot2 z1(M_PI_4 + 0.1); // h(x) - z = -0.1 + double sigma1 = 0.1; + Graph::sharedFactor factor1(new BearingMeasurement(z1, sigma1, 2, 3)); + CHECK(assert_equal(Vector_(1,-0.1),factor1->error_vector(c))); + G.push_back(factor1); + + // Create range factor + double z2(sqrt(2) - 0.22); // h(x) - z = 0.22 + double sigma2 = 0.1; + Graph::sharedFactor factor2(new RangeMeasurement(z2, sigma2, 2, 3)); + CHECK(assert_equal(Vector_(1,0.22),factor2->error_vector(c))); + G.push_back(factor2); + + CHECK(assert_equal(Vector_(2,-0.1,0.22),G.error_vector(c))); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/cpp/testRot2.cpp b/cpp/testRot2.cpp index 54c495ff6..289bee544 100644 --- a/cpp/testRot2.cpp +++ b/cpp/testRot2.cpp @@ -28,20 +28,20 @@ TEST( Rot2, transpose) /* ************************************************************************* */ TEST( Rot2, negtranspose) { - CHECK(assert_equal(-inverse(R).matrix(),R.negtranspose())); + CHECK(assert_equal(-inverse(R).matrix(),R.negtranspose())); } /* ************************************************************************* */ TEST( Rot2, compose) { - CHECK(assert_equal(Rot2(0.45), Rot2(0.2)*Rot2(0.25))); - CHECK(assert_equal(Rot2(0.45), Rot2(0.25)*Rot2(0.2))); + CHECK(assert_equal(Rot2(0.45), Rot2(0.2)*Rot2(0.25))); + CHECK(assert_equal(Rot2(0.45), Rot2(0.25)*Rot2(0.2))); } /* ************************************************************************* */ TEST( Rot2, invcompose) { - CHECK(assert_equal(Rot2(0.2), invcompose(Rot2(0.25),Rot2(0.45)))); + CHECK(assert_equal(Rot2(0.2), invcompose(Rot2(0.25),Rot2(0.45)))); } /* ************************************************************************* */ @@ -62,54 +62,39 @@ TEST( Rot2, expmap) /* ************************************************************************* */ TEST(Rot2, logmap) { - Rot2 rot0(M_PI_2); - Rot2 rot(M_PI); - Vector expected = Vector_(1, M_PI_2); - Vector actual = logmap(rot0,rot); - CHECK(assert_equal(expected, actual)); + Rot2 rot0(M_PI_2); + Rot2 rot(M_PI); + Vector expected = Vector_(1, M_PI_2); + Vector actual = logmap(rot0, rot); + CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ -// rotate derivatives - -TEST( Rot2, Drotate1) +// rotate and derivatives +inline Point2 rotate_(const Rot2 & R, const Point2& p) {return R.rotate(p);} +TEST( Rot2, rotate) { - Matrix computed = Drotate1(R, P); - Matrix numerical = numericalDerivative21(rotate, R, P); - CHECK(assert_equal(numerical,computed)); -} - -TEST( Rot2, Drotate2_DNrotate2) -{ - Matrix computed = Drotate2(R); - Matrix numerical = numericalDerivative22(rotate, R, P); - CHECK(assert_equal(numerical,computed)); + Matrix H1, H2; + Point2 actual = rotate(R, P, H1, H2); + CHECK(assert_equal(actual,R*P)); + Matrix numerical1 = numericalDerivative21(rotate_, R, P); + CHECK(assert_equal(numerical1,H1)); + Matrix numerical2 = numericalDerivative22(rotate_, R, P); + CHECK(assert_equal(numerical2,H2)); } /* ************************************************************************* */ -// unrotate - +// unrotate and derivatives +inline Point2 unrotate_(const Rot2 & R, const Point2& p) {return R.unrotate(p);} TEST( Rot2, unrotate) { - Point2 w = R * P; - CHECK(assert_equal(unrotate(R,w),P)); -} - -/* ************************************************************************* */ -// unrotate derivatives - -TEST( Rot2, Dunrotate1) -{ - Matrix computed = Dunrotate1(R, P); - Matrix numerical = numericalDerivative21(unrotate, R, P); - CHECK(assert_equal(numerical,computed)); -} - -TEST( Rot2, Dunrotate2_DNunrotate2) -{ - Matrix computed = Dunrotate2(R); - Matrix numerical = numericalDerivative22(unrotate, R, P); - CHECK(assert_equal(numerical,computed)); + Matrix H1, H2; + Point2 w = R * P, actual = unrotate(R, w, H1, H2); + CHECK(assert_equal(actual,P)); + Matrix numerical1 = numericalDerivative21(unrotate_, R, w); + CHECK(assert_equal(numerical1,H1)); + Matrix numerical2 = numericalDerivative22(unrotate_, R, w); + CHECK(assert_equal(numerical2,H2)); } /* ************************************************************************* */