diff --git a/gtsam.h b/gtsam.h index f9a7483bb..320d140ea 100644 --- a/gtsam.h +++ b/gtsam.h @@ -266,23 +266,12 @@ class Point2 { // Group static gtsam::Point2 identity(); - gtsam::Point2 inverse() const; - gtsam::Point2 compose(const gtsam::Point2& p2) const; - gtsam::Point2 between(const gtsam::Point2& p2) const; - - // Manifold - gtsam::Point2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Point2& p) const; - - // Lie Group - static gtsam::Point2 Expmap(Vector v); - static Vector Logmap(const gtsam::Point2& p); // Standard Interface double x() const; double y() const; Vector vector() const; - double dist(const gtsam::Point2& p2) const; + double distance(const gtsam::Point2& p2) const; double norm() const; // enabling serialization functionality diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index bc6132812..9327b91e8 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -29,15 +29,15 @@ void Point2::print(const string& s) const { /* ************************************************************************* */ bool Point2::equals(const Point2& q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); + return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol); } /* ************************************************************************* */ double Point2::norm(OptionalJacobian<1,2> H) const { - double r = sqrt(x_ * x_ + y_ * y_); + double r = std::sqrt(x() * x() + y() * y()); if (H) { if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r; + *H << x() / r, y() / r; else *H << 1, 1; // really infinity, why 1 ? } @@ -83,7 +83,7 @@ boost::optional Point2::CircleCircleIntersection(double R_d, double r_d, // Hence, there are only solutions if >=0 if (h2<-tol) return boost::none; // allow *slightly* negative else if (h2 Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) { // distance between circle centers. - double d = c1.dist(c2); + double d = c1.distance(c2); // centers coincide, either no solution or infinite number of solutions. if (d<1e-9) return list(); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 3099a8bb3..4e556746a 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -29,31 +29,32 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Point2 { +class GTSAM_EXPORT Point2 : public Vector2 { private: - double x_, y_; - public: enum { dimension = 2 }; /// @name Standard Constructors /// @{ /// default constructor - Point2(): x_(0), y_(0) {} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + // Deprecated default constructor initializes to zero, in contrast to new behavior below + Point2() { setZero(); } +#else + Point2() { + throw std::runtime_error("Point2 default"); + } +#endif - /// construct from doubles - Point2(double x, double y): x_(x), y_(y) {} + using Vector2::Vector2; /// @} /// @name Advanced Constructors /// @{ /// construct from 2D vector - explicit Point2(const Vector2& v) { - x_ = v(0); - y_ = v(1); - } + explicit Point2(const Vector2& v):Vector2(v) {} /* * @brief Circle-circle intersection, given normalized radii. @@ -107,21 +108,7 @@ public: /// @{ /// identity - inline static Point2 identity() {return Point2();} - - /// inverse - inline Point2 operator- () const {return Point2(-x_,-y_);} - - /// add vector on right - inline Point2 operator +(const Vector2& v) const { - return Point2(x_ + v[0], y_ + v[1]); - } - - /// add - inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - - /// subtract - inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} + inline static Point2 identity() {return Point2(0,0);} /// @} /// @name Vector Space @@ -137,35 +124,28 @@ public: double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, OptionalJacobian<1,2> H2 = boost::none) const; - /** @deprecated The following function has been deprecated, use distance above */ - inline double dist(const Point2& p2) const { - return (p2 - *this).norm(); - } - - /// multiply with a scalar - inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} - - /// divide by a scalar - inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - /// @} /// @name Standard Interface /// @{ /// equality - inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;} + inline bool operator ==(const Point2& q) const {return x()==q.x() && y()==q.y();} /// get x - double x() const {return x_;} + inline double x() const {return (*this)[0];} /// get y - double y() const {return y_;} + inline double y() const {return (*this)[1];} - /// return vectorized form (column-wise). TODO: why does this function exist? - Vector2 vector() const { return Vector2(x_, y_); } + /// return vectorized form (column-wise). + const Vector2& vector() const { return *this; } /// @} + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p); + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} @@ -177,10 +157,9 @@ public: Point2 retract(const Vector2& v) const { return compose(Point2(v));} static Vector2 Logmap(const Point2& p) { return p.vector();} static Point2 Expmap(const Vector2& v) { return Point2(v);} + inline double dist(const Point2& p2) const {return distance();} /// @} - - /// Streaming - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p); +#endif private: @@ -192,11 +171,9 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(x_); - ar & BOOST_SERIALIZATION_NVP(y_); - } + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector2);} - /// @} + /// @} }; // Convenience typedef @@ -207,10 +184,13 @@ std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); typedef std::vector Point2Vector; /// multiply with scalar -inline Point2 operator*(double s, const Point2& p) {return p*s;} +inline Point2 operator*(double s, const Point2& p) { +return p * s; +} template<> -struct traits : public internal::VectorSpace {}; +struct traits : public internal::VectorSpace { +}; } // \ namespace gtsam diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 34b146bee..49704642e 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -58,16 +58,16 @@ bool Pose2::equals(const Pose2& q, double tol) const { /* ************************************************************************* */ Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) { - if (H) *H = Pose2::ExpmapDerivative(xi); assert(xi.size() == 3); - Point2 v(xi(0),xi(1)); - double w = xi(2); + if (H) *H = Pose2::ExpmapDerivative(xi); + const Point2 v(xi(0),xi(1)); + const double w = xi(2); if (std::abs(w) < 1e-10) return Pose2(xi[0], xi[1], xi[2]); else { - Rot2 R(Rot2::fromAngle(w)); - Point2 v_ortho = R_PI_2 * v; // points towards rot center - Point2 t = (v_ortho - R.rotate(v_ortho)) / w; + const Rot2 R(Rot2::fromAngle(w)); + const Point2 v_ortho = R_PI_2 * v; // points towards rot center + const Point2 t = (v_ortho - R.rotate(v_ortho)) / w; return Pose2(R, t); } } @@ -311,7 +311,7 @@ boost::optional align(const vector& pairs) { if (n<2) return boost::none; // we need at least two pairs // calculate centroids - Point2 cp,cq; + Point2 cp(0,0), cq(0,0); for(const Point2Pair& pair: pairs) { cp += pair.first; cq += pair.second; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 31dfb479f..b4a618db6 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -52,7 +52,9 @@ public: /// @{ /** default constructor = origin */ - Pose2() {} // default is origin + Pose2() : + r_(traits::Identity()), t_(traits::Identity()) { + } /** copy constructor */ Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 9f21bb82a..cd05af519 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -78,7 +78,7 @@ public: /// Construct from 2D point in plane at focal length f /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point - explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) { + explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) { p_.normalize(); } diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index cc7a0c0f8..c7e4e3ae7 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -152,7 +152,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity); - CHECK(assert_equal(Point2(), result)); + CHECK(assert_equal(Point2(0,0), result)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 0afa04411..478f5baf4 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -44,7 +44,7 @@ TEST(CameraSet, Pinhole) { EXPECT(!set.equals(set2)); // Check measurements - Point2 expected; + Point2 expected(0,0); ZZ z = set.project2(p); EXPECT(assert_equal(expected, z[0])); EXPECT(assert_equal(expected, z[1])); @@ -117,7 +117,7 @@ TEST(CameraSet, Pinhole) { Unit3 pointAtInfinity(0, 0, 1000); EXPECT( assert_equal(pointAtInfinity, - camera.backprojectPointAtInfinity(Point2()))); + camera.backprojectPointAtInfinity(Point2(0,0)))); actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E); EXPECT(assert_equal(expectedV, actualV)); LONGS_EQUAL(2, Fs.size()); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 99dcb95bf..5cca1d723 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -153,12 +153,12 @@ TEST( PinholeCamera, backproject2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backproject(Point2(), 1.); + Point3 actual = camera.backproject(Point2(0,0), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x.first)); + EXPECT(assert_equal(Point2(0,0), x.first)); EXPECT(x.second); } @@ -169,12 +169,12 @@ TEST( PinholeCamera, backprojectInfinity2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0)); Unit3 expected(0., 1., 0.); Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x)); + EXPECT(assert_equal(Point2(0,0), x)); } /* ************************************************************************* */ @@ -184,12 +184,12 @@ TEST( PinholeCamera, backprojectInfinity3) Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); - Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0)); Unit3 expected(0., 0., 1.); Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x)); + EXPECT(assert_equal(Point2(0,0), x)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 0d840de7e..0caca3fda 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -124,12 +124,12 @@ TEST( PinholePose, backproject2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backproject(Point2(), 1.); + Point3 actual = camera.backproject(Point2(0,0), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x.first)); + EXPECT(assert_equal(Point2(0,0), x.first)); EXPECT(x.second); } diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp index b8f001f1c..6b20ec7bd 100644 --- a/gtsam/geometry/tests/testPinholeSet.cpp +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -131,7 +131,7 @@ TEST(PinholeSet, Pinhole) { } EXPECT( assert_equal(pointAtInfinity, - camera.backprojectPointAtInfinity(Point2()))); + camera.backprojectPointAtInfinity(Point2(0,0)))); { PinholeSet::FBlocks Fs; Matrix E; diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 3a636b9bf..87deba16c 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -27,6 +27,11 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point2) GTSAM_CONCEPT_LIE_INST(Point2) +//****************************************************************************** +TEST(Point2 , Constructor) { + Point2 p; +} + //****************************************************************************** TEST(Double , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -95,12 +100,12 @@ TEST( Point2, expmap) { /* ************************************************************************* */ TEST( Point2, arithmetic) { - EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); - EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); - EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); - EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2)); - EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3))); - EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); + EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); + EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); + EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); + EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2)); + EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3))); + EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 9b6e53323..60fff30cc 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -26,6 +26,11 @@ GTSAM_CONCEPT_LIE_INST(Point3) static Point3 P(0.2, 0.7, -2); +//****************************************************************************** +TEST(Point3 , Constructor) { + Point3 p; +} + //****************************************************************************** TEST(Point3 , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index bccfdc1c7..ca716cb80 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -43,7 +43,7 @@ TEST(Pose2 , Concept) { /* ************************************************************************* */ TEST(Pose2, constructors) { - Point2 p; + Point2 p(0,0); Pose2 pose(0,p); Pose2 origin; assert_equal(pose,origin); @@ -371,7 +371,7 @@ TEST(Pose2, compose_c) /* ************************************************************************* */ TEST(Pose2, inverse ) { - Point2 origin, t(1,2); + Point2 origin(0,0), t(1,2); Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y Pose2 identity, lTg = gTl.inverse(); @@ -409,7 +409,7 @@ namespace { /* ************************************************************************* */ TEST( Pose2, matrix ) { - Point2 origin, t(1,2); + Point2 origin(0,0), t(1,2); Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y Matrix gMl = matrix(gTl); EXPECT(assert_equal((Matrix(3,3) << diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 70b3069f2..edf122d3c 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -104,12 +104,12 @@ TEST( SimpleCamera, backproject2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down SimpleCamera camera(Pose3(rot, origin), K); - Point3 actual = camera.backproject(Point2(), 1.); + Point3 actual = camera.backproject(Point2(0,0), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x.first)); + CHECK(assert_equal(Point2(0,0), x.first)); CHECK(x.second); } diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index c3df95abc..86b7734f7 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -273,7 +273,7 @@ TEST( triangulation, onePose) { vector measurements; poses += Pose3(); - measurements += Point2(); + measurements += Point2(0,0); CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), TriangulationUnderconstrainedException); @@ -282,7 +282,7 @@ TEST( triangulation, onePose) { //****************************************************************************** TEST( triangulation, StereotriangulateNonlinear ) { - Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + auto stereoK = boost::make_shared(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); // two camera poses m1, m2 Matrix4 m1, m2; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index dece9bad8..6e4a1b58c 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -80,7 +80,7 @@ TEST(Expression, Leaves) { // Unary(Leaf) namespace unary { Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) { - return Point2(); + return Point2(0,0); } double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index e97cd2730..166c241b3 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -83,7 +83,6 @@ public: "TriangulationFactor must be created with " + boost::lexical_cast((int) Measurement::dimension) + "-dimensional noise model."); - } /** Virtual destructor */ @@ -160,7 +159,7 @@ public: // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project2(point, boost::none, A) - measured_).vector(); + b = traits::Local(camera_.project2(point, boost::none, A), measured_); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 96052bd0f..f69f4c113 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -55,8 +55,8 @@ struct traits : public Testable {}; TEST(SmartFactorBase, Pinhole) { PinholeFactor f= PinholeFactor(unit2); - f.add(Point2(), 1); - f.add(Point2(), 2); + f.add(Point2(0,0), 1); + f.add(Point2(0,0), 2); EXPECT_LONGS_EQUAL(2 * 2, f.dim()); } diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index f610e4d0f..610fddfbf 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -260,7 +260,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -272,7 +272,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -285,7 +285,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -303,7 +303,7 @@ Point2 SimPolygon2D::randomBoundedPoint2( return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 0447f2e39..7989e2775 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -101,7 +101,7 @@ public: // loop over all circles for(const Circle2& it: circles) { // distance between circle centers. - double d = circle1.center.dist(it.center); + double d = circle1.center.distance(it.center); if (d < 1e-9) continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles @@ -123,8 +123,8 @@ public: double error1 = 0, error2 = 0; Point2 p1 = intersections.front(), p2 = intersections.back(); for(const Circle2& it: circles) { - error1 += it.center.dist(p1); - error2 += it.center.dist(p2); + error1 += it.center.distance(p1); + error2 += it.center.distance(p2); } return (error1 < error2) ? p1 : p2; //gttoc_(triangulate); diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index aae4e413d..68d37a070 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -99,12 +99,12 @@ public: *H3 = D_e_point_g * D_point_g_base2; if (H4) *H4 = D_e_point_g * D_point_g_point; - return (d - measured_).vector(); + return d - measured_; } else { Pose2 pose_g = base1.compose(pose); Point2 point_g = base2.transform_from(point); Point2 d = pose_g.transform_to(point_g); - return (d - measured_).vector(); + return d - measured_; } } }; diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp index 7af3f8cf6..dad88e077 100644 --- a/python/handwritten/geometry/Point2.cpp +++ b/python/handwritten/geometry/Point2.cpp @@ -28,6 +28,7 @@ using namespace gtsam; BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(distance_overloads, Point2::distance, 1, 3) void exportPoint2(){ @@ -35,13 +36,12 @@ void exportPoint2(){ .def(init()) .def(init()) .def("identity", &Point2::identity) - .def("dist", &Point2::dist) - .def("distance", &Point2::distance) + .def("distance", &Point2::distance, distance_overloads(args("q","H1","H2"))) .def("equals", &Point2::equals, equals_overloads(args("q","tol"))) .def("norm", &Point2::norm) .def("print", &Point2::print, print_overloads(args("s"))) .def("unit", &Point2::unit) - .def("vector", &Point2::vector) + .def("vector", &Point2::vector, return_value_policy()) .def("x", &Point2::x) .def("y", &Point2::y) .def(self * other()) // __mult__ @@ -55,4 +55,4 @@ void exportPoint2(){ .def(self == self) ; -} \ No newline at end of file +} diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 0012f5f45..3245652be 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -140,7 +140,7 @@ namespace simulated2D { /// Return error and optional derivative Vector evaluateError(const Pose& x, boost::optional H = boost::none) const { - return (prior(x, H) - measured_).vector(); + return (prior(x, H) - measured_); } virtual ~GenericPrior() {} @@ -186,7 +186,7 @@ namespace simulated2D { Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (odo(x1, x2, H1, H2) - measured_).vector(); + return (odo(x1, x2, H1, H2) - measured_); } virtual ~GenericOdometry() {} @@ -233,7 +233,7 @@ namespace simulated2D { Vector evaluateError(const Pose& x1, const Landmark& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - measured_).vector(); + return (mea(x1, x2, H1, H2) - measured_); } virtual ~GenericMeasurement() {} diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index ccc734cfd..b698cfeaa 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -111,7 +111,7 @@ namespace simulated2D { * @return a scalar distance between values */ template - double range_trait(const T1& a, const T2& b) { return a.dist(b); } + double range_trait(const T1& a, const T2& b) { return a.distance(b); } /** * Binary inequality constraint forcing the range between points diff --git a/tests/smallExample.h b/tests/smallExample.h index 215655593..e40ba6079 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -337,7 +337,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { if (A) *A = H(x); - return (h(x) - z_).vector(); + return (h(x) - z_); } }; @@ -593,7 +593,7 @@ inline boost::tuple planarGraph(size_t N) { Values zeros; for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) - zeros.insert(key(x, y), Point2()); + zeros.insert(key(x, y), Point2(0,0)); VectorValues xtrue; for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 4fda27cdb..b6b196acc 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -193,7 +193,7 @@ TEST(ExpressionFactor, Binary) { internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; Point2 value = binary.traceExecution(values, trace, traceStorage); - EXPECT(assert_equal(Point2(),value, 1e-9)); + EXPECT(assert_equal(Point2(0,0),value, 1e-9)); // trace.print(); // Expected Jacobians @@ -248,7 +248,7 @@ TEST(ExpressionFactor, Shallow) { internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, traceStorage); - EXPECT(assert_equal(Point2(),value, 1e-9)); + EXPECT(assert_equal(Point2(0,0),value, 1e-9)); // trace.print(); // Expected Jacobians diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 00ab4a16c..debc82a4e 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -226,7 +226,7 @@ public: *H2 = Matrix::Identity(dim(),dim()); // Return the error between the prediction and the supplied value of p2 - return (p2 - prediction).vector(); + return (p2 - prediction); } }; diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 65d26eb98..d31ddbbef 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -78,7 +78,7 @@ TEST(Manifold, Identity) { EXPECT_DOUBLES_EQUAL(0.0, traits::Identity(), 0.0); EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::Identity()))); EXPECT(assert_equal(Pose3(), traits::Identity())); - EXPECT(assert_equal(Point2(), traits::Identity())); + EXPECT(assert_equal(Point2(0,0), traits::Identity())); } //****************************************************************************** diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 86080b673..95843e5ab 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -417,7 +417,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { graph.push_back(factor); Values initValues; - initValues.insert(key1, Point2()); + initValues.insert(key1, Point2(0,0)); initValues.insert(key2, badPt); Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); @@ -454,7 +454,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Values initialEstimate; initialEstimate.insert(x1, pt_x1); - initialEstimate.insert(x2, Point2()); + initialEstimate.insert(x2, Point2(0,0)); initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 3c49d54af..617a8cc1c 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -117,9 +117,9 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { new_factors += PriorFactor(lm3, landmark3, model2); // Initialize to origin - new_init.insert(lm1, Point2()); - new_init.insert(lm2, Point2()); - new_init.insert(lm3, Point2()); + new_init.insert(lm1, Point2(0,0)); + new_init.insert(lm2, Point2(0,0)); + new_init.insert(lm3, Point2(0,0)); } isamChol.update(new_factors, new_init); @@ -194,9 +194,9 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { new_factors += PriorFactor(lm3, landmark3, model2); // Initialize to origin - new_init.insert(lm1, Point2()); - new_init.insert(lm2, Point2()); - new_init.insert(lm3, Point2()); + new_init.insert(lm1, Point2(0,0)); + new_init.insert(lm2, Point2(0,0)); + new_init.insert(lm3, Point2(0,0)); } // Reconnect with observation later