diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 86e01d71e..026becebe 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -24,7 +24,7 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Matrix26 PinholeBase::Dpose(const Vector2& pn, double d) { +Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { // optimized version of derivatives, see CalibratedCamera.nb const double u = pn.x(), v = pn.y(); double uv = u * v, uu = u * u, vv = v * v; @@ -34,7 +34,7 @@ Matrix26 PinholeBase::Dpose(const Vector2& pn, double d) { } /* ************************************************************************* */ -Matrix23 PinholeBase::Dpoint(const Vector2& pn, double d, const Matrix3& Rt) { +Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) { // optimized version of derivatives, see CalibratedCamera.nb const double u = pn.x(), v = pn.y(); Matrix23 Dpn_point; @@ -85,20 +85,20 @@ const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const { } /* ************************************************************************* */ -Vector2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { +Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { double d = 1.0 / pc.z(); const double u = pc.x() * d, v = pc.y() * d; if (Dpoint) *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Vector2(u, v); + return Point2(u, v); } /* ************************************************************************* */ -Vector2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { +Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { if (Dpoint) { Matrix32 Dpoint3_pc; Matrix23 Duv_point3; - Vector2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3); + Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3); *Dpoint = Duv_point3 * Dpoint3_pc; return uv; } else @@ -106,14 +106,14 @@ Vector2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { } /* ************************************************************************* */ -pair PinholeBase::projectSafe(const Point3& pw) const { +pair PinholeBase::projectSafe(const Point3& pw) const { const Point3 pc = pose().transform_to(pw); - const Vector2 pn = Project(pc); + const Point2 pn = Project(pc); return make_pair(pn, pc.z() > 0); } /* ************************************************************************* */ -Vector2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, +Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { Matrix3 Rt; // calculated by transform_to if needed @@ -122,7 +122,7 @@ Vector2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, if (q.z() <= 0) throw CheiralityException(); #endif - const Vector2 pn = Project(q); + const Point2 pn = Project(q); if (Dpose || Dpoint) { const double d = 1.0 / q.z(); @@ -135,7 +135,7 @@ Vector2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, } /* ************************************************************************* */ -Vector2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, +Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 2> Dpoint) const { // world to camera coordinate @@ -146,7 +146,7 @@ Vector2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, // camera to normalized image coordinate Matrix2 Dpn_pc; - const Vector2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); + const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); // chain the Jacobian matrices if (Dpose) { @@ -161,7 +161,7 @@ Vector2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, return pn; } /* ************************************************************************* */ -Point3 PinholeBase::backproject_from_camera(const Vector2& p, +Point3 PinholeBase::backproject_from_camera(const Point2& p, const double depth) { return Point3(p.x() * depth, p.y() * depth, depth); } @@ -178,7 +178,7 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, } /* ************************************************************************* */ -Vector2 CalibratedCamera::project(const Point3& point, +Point2 CalibratedCamera::project(const Point3& point, OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { return project2(point, Dcamera, Dpoint); } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index bba438e70..81f448a7c 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -54,7 +54,7 @@ public: * Some classes template on either PinholeCamera or StereoCamera, * and this typedef informs those classes what "project" returns. */ - typedef Vector2 Measurement; + typedef Point2 Measurement; private: @@ -70,7 +70,7 @@ protected: * @param pn projection in normalized coordinates * @param d disparity (inverse depth) */ - static Matrix26 Dpose(const Vector2& pn, double d); + static Matrix26 Dpose(const Point2& pn, double d); /** * Calculate Jacobian with respect to point @@ -78,7 +78,7 @@ protected: * @param d disparity (inverse depth) * @param Rt transposed rotation matrix */ - static Matrix23 Dpoint(const Vector2& pn, double d, const Matrix3& Rt); + static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt); /// @} @@ -169,7 +169,7 @@ public: * Does *not* throw a CheiralityException, even if pc behind image plane * @param pc point in camera coordinates */ - static Vector2 Project(const Point3& pc, // + static Point2 Project(const Point3& pc, // OptionalJacobian<2, 3> Dpoint = boost::none); /** @@ -177,18 +177,18 @@ public: * Does *not* throw a CheiralityException, even if pc behind image plane * @param pc point in camera coordinates */ - static Vector2 Project(const Unit3& pc, // + static Point2 Project(const Unit3& pc, // OptionalJacobian<2, 2> Dpoint = boost::none); /// Project a point into the image and check depth - std::pair projectSafe(const Point3& pw) const; + std::pair projectSafe(const Point3& pw) const; /** Project point into the image * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point */ - Vector2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = + Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; /** Project point at infinity into the image @@ -196,12 +196,12 @@ public: * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point */ - Vector2 project2(const Unit3& point, + Point2 project2(const Unit3& point, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth - static Point3 backproject_from_camera(const Vector2& p, const double depth); + static Point3 backproject_from_camera(const Point2& p, const double depth); /// @} /// @name Advanced interface @@ -325,11 +325,11 @@ public: * @deprecated * Use project2, which is more consistently named across Pinhole cameras */ - Vector2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = + Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth - Point3 backproject(const Vector2& pn, double depth) const { + Point3 backproject(const Point2& pn, double depth) const { return pose().transform_from(backproject_from_camera(pn, depth)); } diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index dbbe898ef..e1c5b4c4a 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -63,7 +63,7 @@ bool Point2::equals(const Point2& q, double tol) const { /* ************************************************************************* */ double Point2::norm(OptionalJacobian<1,2> H) const { - return norm(*this, H); + return gtsam::norm2(*this, H); } /* ************************************************************************* */ @@ -78,6 +78,18 @@ ostream &operator<<(ostream &os, const Point2& p) { return os; } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +boost::optional CircleCircleIntersection(double R_d, double r_d, double tol) { + return circleCircleIntersection(R_d, r_d, tol); +} +std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh) { + return circleCircleIntersection(c1, c2, fh); +} +std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) { + return circleCircleIntersection(c1, r1, c2, r2, tol); +} +#endif + #endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS /* ************************************************************************* */ diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 8d75c805b..fb250df6d 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -61,14 +61,6 @@ public: /// construct from 2D vector explicit Point2(const Vector2& v):Vector2(v) {} - /// @} - /// @name Declare circle intersection functionality - /// @{ - - friend boost::optional circleCircleIntersection(double R_d, double r_d, double tol); - friend std::list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); - friend std::list circleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol); - /// @} /// @name Testable /// @{ @@ -132,15 +124,9 @@ public: static Vector2 Logmap(const Point2& p) { return p;} static Point2 Expmap(const Vector2& v) { return Point2(v);} inline double dist(const Point2& p2) const {return distance(p2);} - static boost::optional CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9) { - return circleCircleIntersection( R_d, r_d, tol); - } - static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional) { - return circleCircleIntersection( c1, c2, boost::optional); - } - static std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9) { - return CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9); - } + static boost::optional CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9); + static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); + static std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9); /// @} #endif diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index d158c91f0..7dc6bd704 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -34,11 +34,11 @@ void Point3::print(const string& s) const { /* ************************************************************************* */ double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { - return gtsam::distance(*this,q,H1,H2); + return gtsam::distance3(*this,q,H1,H2); } double Point3::norm(OptionalJacobian<1,3> H) const { - return gtsam::norm(*this, H); + return gtsam::norm3(*this, H); } Point3 Point3::normalized(OptionalJacobian<3,3> H) const { diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 7ed8cfc6e..e2a5bcdea 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -111,9 +111,9 @@ TEST( Point2, arithmetic) { /* ************************************************************************* */ TEST( Point2, unit) { Point2 p0(10, 0), p1(0, -10), p2(10, 10); - EXPECT(assert_equal(Point2(1, 0), p0.normalized(), 1e-6)); - EXPECT(assert_equal(Point2(0,-1), p1.normalized(), 1e-6)); - EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.normalized(), 1e-6)); + EXPECT(assert_equal(Point2(1, 0), Point2(p0.normalized()), 1e-6)); + EXPECT(assert_equal(Point2(0,-1), Point2(p1.normalized()), 1e-6)); + EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), Point2(p2.normalized()), 1e-6)); } namespace {