diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 29d59d9e3..8b44583a6 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -33,45 +33,54 @@ void Point3::print(const string& s) 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_); } -/* ************************************************************************* */ Point3 Point3::operator-(const Point3& q) const { return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); } -/* ************************************************************************* */ Point3 Point3::operator*(double s) const { return Point3(x_ * s, y_ * s, z_ * s); } -/* ************************************************************************* */ Point3 Point3::operator/(double s) const { return Point3(x_ / s, y_ / s, z_ / s); } /* ************************************************************************* */ -double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1, +double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { - double d = (p2 - *this).norm(); - if (H1) { - *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z(); - *H1 = *H1 *(1. / d); - } + return gtsam::distance(*this,q,H1,H2); +} - if (H2) { - *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z(); - *H2 = *H2 *(1. / d); - } - return d; +double Point3::norm(OptionalJacobian<1,3> H) const { + return gtsam::norm(*this, H); +} + +Point3 Point3::normalized(OptionalJacobian<3,3> H) const { + return gtsam::normalize(*this, H); +} + +Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + return gtsam::cross(*this, q, H1, H2); +} + +double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) const { + return gtsam::dot(*this, q, H1, H2); +} + +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point3& p) { + os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';"; + return os; } /* ************************************************************************* */ @@ -92,59 +101,56 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, #endif /* ************************************************************************* */ -Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const { - if (H_p) { - *H_p << skewSymmetric(-q.vector()); +double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) { + double d = (q - p1).norm(); + if (H1) { + *H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z(); + *H1 = *H1 *(1. / d); } - if (H_q) { - *H_q << skewSymmetric(vector()); + if (H2) { + *H2 << -p1.x() + q.x(), -p1.y() + q.y(), -p1.z() + q.z(); + *H2 = *H2 *(1. / d); } - - return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, - x_ * q.y_ - y_ * q.x_); + return d; } -/* ************************************************************************* */ -double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const { - if (H_p) { - *H_p << q.vector().transpose(); - } - if (H_q) { - *H_q << vector().transpose(); - } - - return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); -} - -/* ************************************************************************* */ -double Point3::norm(OptionalJacobian<1,3> H) const { - double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); +double norm(const Point3 &p, OptionalJacobian<1, 3> H) { + double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z()); if (H) { if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r, z_ / r; + *H << p.x() / r, p.y() / r, p.z() / r; else - *H << 1, 1, 1; // really infinity, why 1 ? + *H << 1, 1, 1; // really infinity, why 1 ? } return r; } -/* ************************************************************************* */ -Point3 Point3::normalize(OptionalJacobian<3,3> H) const { - Point3 normalized = *this / norm(); +Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) { + Point3 normalized = p / norm(p); 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 << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z(); + double xy = p.x() * p.y(), xz = p.x() * p.z(), yz = p.y() * p.z(); + *H << y2 + z2, -xy, -xz, /**/ -xy, x2 + z2, -yz, /**/ -xz, -yz, x2 + y2; *H /= pow(x2 + y2 + z2, 1.5); } return normalized; } -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Point3& p) { - os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';"; - return os; +Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) { + if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z()); + if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z()); + return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(), + p.x() * q.y() - p.y() * q.x()); +} + +double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) { + if (H1) *H1 << q.x(), q.y(), q.z(); + if (H2) *H2 << p.x(), p.y(), p.z(); + return p.x() * q.x() + p.y() * q.y() + p.z() * q.z(); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 978d7b963..90bfb7cea 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -104,14 +104,14 @@ namespace gtsam { Point3 operator / (double s) const; /** distance between two points */ - double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, + double distance(const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) const; /** Distance of the point from the origin, with Jacobian */ double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; + Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // @@ -155,7 +155,8 @@ namespace gtsam { Point3 retract(const Vector3& v) const { return compose(Point3(v));} static Vector3 Logmap(const Point3& p) { return p.vector();} static Point3 Expmap(const Vector3& v) { return Point3(v);} - inline double dist(const Point3& p2) const { return (p2 - *this).norm(); } + inline double dist(const Point3& q) const { return (q - *this).norm(); } + Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);} Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, @@ -194,6 +195,31 @@ struct traits : public internal::VectorSpace {}; template<> struct traits : public internal::VectorSpace {}; +/// distance between two points +double distance(const Point3& p1, const Point3& q, + OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none); + +/// Distance of the point from the origin, with Jacobian +double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none); + +/// normalize, with optional Jacobian +Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); + +/// cross product @return this x q +Point3 cross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H_p = boost::none, + OptionalJacobian<3, 3> H_q = boost::none); + +/// dot product +double dot(const Point3& p, const Point3& q, + OptionalJacobian<1, 3> H_p = boost::none, + OptionalJacobian<1, 3> H_q = boost::none); + +// Convenience typedef +typedef std::pair Point3Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); + template struct Range; @@ -203,7 +229,7 @@ struct Range { double operator()(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) { - return p.distance(q, H1, H2); + return distance(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index aaf0aa953..d80fe2915 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -44,7 +44,7 @@ namespace gtsam { Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix3 D_p_point; - Point3 normalized = point.normalize(H ? &D_p_point : 0); + Point3 normalized = normalize(point, H ? &D_p_point : 0); Unit3 direction; direction.p_ = normalized.vector(); if (H) @@ -108,7 +108,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Normalize result to get a unit vector: b1 = B1 / |B1|. Matrix33 H_b1_B1; - Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0); + Point3 b1 = normalize(B1, H ? &H_b1_B1 : 0); // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // No need to normalize this, p and b1 are orthogonal unit vectors. diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index d0123af83..37ad105c7 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -96,9 +96,9 @@ TEST (Point3, normalize) { Matrix actualH; Point3 point(1, -2, 3); // arbitrary point Point3 expected(point / sqrt(14.0)); - EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); + EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(&Point3::normalize, _1, boost::none), point); + boost::bind(gtsam::normalize, _1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index 5a3ba8fb5..19f5a4602 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -41,7 +41,7 @@ class_("Point3") .def("dot", &Point3::dot) .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) .def("norm", &Point3::norm) - .def("normalize", &Point3::normalize) + .def("normalized", &Point3::normalized) .def("print", &Point3::print, print_overloads(args("s"))) .def("vector", &Point3::vector) .def("x", &Point3::x)