From 2acffe885e3942a97ee2b85b6f529cdfd6a5387d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 5 Jan 2014 16:25:14 -0500 Subject: [PATCH] normalize --- gtsam/geometry/Point3.cpp | 54 ++++++++++++++-------- gtsam/geometry/Point3.h | 3 ++ gtsam/geometry/tests/testPoint3.cpp | 71 +++++++++++++++++------------ 3 files changed, 80 insertions(+), 48 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 4f5f43665..b6244630e 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3); /* ************************************************************************* */ bool Point3::equals(const Point3 & q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol + && fabs(z_ - q.z()) < tol); } /* ************************************************************************* */ @@ -37,18 +38,18 @@ void Point3::print(const string& s) const { /* ************************************************************************* */ -bool Point3::operator== (const Point3& q) const { +bool Point3::operator==(const Point3& q) const { return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; } /* ************************************************************************* */ Point3 Point3::operator+(const Point3& q) const { - return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ ); + return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_); } /* ************************************************************************* */ -Point3 Point3::operator- (const Point3& q) const { - return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); +Point3 Point3::operator-(const Point3& q) const { + return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); } /* ************************************************************************* */ @@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const { } /* ************************************************************************* */ -Point3 Point3::add(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = eye(3,3); +Point3 Point3::add(const Point3 &q, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = eye(3, 3); + if (H2) + *H2 = eye(3, 3); return *this + q; } /* ************************************************************************* */ -Point3 Point3::sub(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = -eye(3,3); +Point3 Point3::sub(const Point3 &q, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = eye(3, 3); + if (H2) + *H2 = -eye(3, 3); return *this - q; } /* ************************************************************************* */ Point3 Point3::cross(const Point3 &q) const { - return Point3( y_*q.z_ - z_*q.y_, - z_*q.x_ - x_*q.z_, - x_*q.y_ - y_*q.x_ ); + return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, + x_ * q.y_ - y_ * q.x_); } /* ************************************************************************* */ double Point3::dot(const Point3 &q) const { - return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); + return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); } /* ************************************************************************* */ double Point3::norm() const { - return sqrt( x_*x_ + y_*y_ + z_*z_ ); + return sqrt(x_ * x_ + y_ * y_ + z_ * z_); +} + +/* ************************************************************************* */ +Point3 Point3::normalize(boost::optional H) const { + Point3 normalized = *this / norm(); + if (H) { + // 3*3 Derivative + double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; + double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; + H->resize(3, 3); + *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + *H /= pow(x2 + y2 + z2, 1.5); + } + return normalized; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 5e2e0ff42..66924ec07 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -176,6 +176,9 @@ namespace gtsam { /** Distance of the point from the origin */ double norm() const; + /** normalize, with optional Jacobian */ + Point3 normalize(boost::optional H = boost::none) const; + /** cross product @return this x q */ Point3 cross(const Point3 &q) const; diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index ef4a3894c..58c59e83d 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -12,75 +12,86 @@ /** * @file testPoint3.cpp * @brief Unit tests for Point3 class - */ + */ -#include -#include #include +#include +#include +#include using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point3) GTSAM_CONCEPT_LIE_INST(Point3) -static Point3 P(0.2,0.7,-2); +static Point3 P(0.2, 0.7, -2); /* ************************************************************************* */ TEST(Point3, Lie) { - Point3 p1(1,2,3); - Point3 p2(4,5,6); + Point3 p1(1, 2, 3); + Point3 p2(4, 5, 6); Matrix H1, H2; - EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2))); EXPECT(assert_equal(eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2))); + EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2))); EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.)))); - EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.)))); + EXPECT(assert_equal(Vector_(3, 3., 3., 3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ -TEST( Point3, arithmetic) -{ - CHECK(P*3==3*P); - CHECK(assert_equal( Point3(-1,-5,-6), -Point3(1,5,6) )); - CHECK(assert_equal( Point3(2,5,6), Point3(1,4,5)+Point3(1,1,1))); - CHECK(assert_equal( Point3(0,3,4), Point3(1,4,5)-Point3(1,1,1))); - CHECK(assert_equal( Point3(2,8,6), Point3(1,4,3)*2)); - CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3))); - CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2)); +TEST( Point3, arithmetic) { + CHECK(P * 3 == 3 * P); + CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); + CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); + CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); + CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); + CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); + CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); } /* ************************************************************************* */ -TEST( Point3, equals) -{ +TEST( Point3, equals) { CHECK(P.equals(P)); Point3 Q; CHECK(!P.equals(Q)); } /* ************************************************************************* */ -TEST( Point3, dot) -{ - Point3 origin, ones(1,1,1); - CHECK(origin.dot(Point3(1,1,0)) == 0); - CHECK(ones.dot(Point3(1,1,0)) == 2); +TEST( Point3, dot) { + Point3 origin, ones(1, 1, 1); + CHECK(origin.dot(Point3(1, 1, 0)) == 0); + CHECK(ones.dot(Point3(1, 1, 0)) == 2); } /* ************************************************************************* */ -TEST( Point3, stream) -{ - Point3 p(1,2, -3); +TEST( Point3, stream) { + Point3 p(1, 2, -3); std::ostringstream os; os << p; EXPECT(os.str() == "[1, 2, -3]';"); } +//************************************************************************* +TEST (Point3, normalize) { + Matrix actualH; + Point3 point(1, -2, 3); // arbitrary point + Point3 expected(point / sqrt(14)); + EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(&Point3::normalize, _1, boost::none), point); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */