From 2b9a3db085cfd7b235f35a0a1217b8af52640e1b Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 5 Nov 2011 23:01:43 +0000 Subject: [PATCH] Added Group concept, reworked naming and conventions to reduce unnecessary functions --- gtsam/base/Group.h | 47 ++ gtsam/base/Lie.h | 54 +-- gtsam/base/LieScalar.h | 72 ++- gtsam/base/LieVector.h | 48 +- gtsam/base/Manifold.h | 21 +- gtsam/base/lieProxies.h | 7 - gtsam/base/numericalDerivative.h | 24 +- gtsam/base/tests/testLieScalar.cpp | 5 +- gtsam/base/tests/testLieVector.cpp | 1 - gtsam/geometry/Cal3Bundler.cpp | 10 +- gtsam/geometry/Cal3Bundler.h | 5 +- gtsam/geometry/Cal3DS2.cpp | 8 +- gtsam/geometry/Cal3DS2.h | 5 +- gtsam/geometry/Cal3_S2.h | 13 +- gtsam/geometry/CalibratedCamera.cpp | 14 +- gtsam/geometry/CalibratedCamera.h | 8 +- gtsam/geometry/CalibratedCameraT.h | 4 +- gtsam/geometry/GeneralCameraT.h | 278 ++++++------ gtsam/geometry/Point2.cpp | 28 +- gtsam/geometry/Point2.h | 189 ++++---- gtsam/geometry/Point3.cpp | 118 ++--- gtsam/geometry/Point3.h | 19 +- gtsam/geometry/Pose2.cpp | 521 +++++++++++----------- gtsam/geometry/Pose2.h | 277 ++++++------ gtsam/geometry/Pose3.cpp | 62 +-- gtsam/geometry/Pose3.h | 19 +- gtsam/geometry/Rot2.h | 25 +- gtsam/geometry/Rot3.h | 19 +- gtsam/geometry/StereoCamera.h | 29 +- gtsam/geometry/StereoPoint2.h | 24 +- gtsam/geometry/tests/testHomography2.cpp | 2 +- gtsam/geometry/tests/testPoint2.cpp | 7 +- gtsam/geometry/tests/testPoint3.cpp | 5 +- gtsam/geometry/tests/testPose2.cpp | 28 +- gtsam/geometry/tests/testPose3.cpp | 27 +- gtsam/geometry/tests/testRot2.cpp | 5 +- gtsam/geometry/tests/testRot3.cpp | 11 +- gtsam/geometry/tests/testStereoPoint2.cpp | 1 - gtsam/nonlinear/LieValues-inl.h | 8 +- gtsam/nonlinear/LieValues.h | 4 +- gtsam/nonlinear/NonlinearConstraint.h | 4 +- gtsam/nonlinear/NonlinearEquality.h | 2 +- gtsam/nonlinear/TupleValues.h | 18 +- gtsam/slam/BetweenConstraint.h | 2 +- gtsam/slam/BetweenFactor.h | 2 +- gtsam/slam/GeneralSFMFactor.h | 2 +- gtsam/slam/PartialPriorFactor.h | 9 +- gtsam/slam/PriorFactor.h | 2 +- gtsam/slam/simulated2DOriented.h | 4 +- gtsam/slam/tests/testPose3SLAM.cpp | 4 +- tests/testExtendedKalmanFilter.cpp | 2 +- tests/testTupleValues.cpp | 6 +- 52 files changed, 970 insertions(+), 1139 deletions(-) create mode 100644 gtsam/base/Group.h diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h new file mode 100644 index 000000000..3c56a541a --- /dev/null +++ b/gtsam/base/Group.h @@ -0,0 +1,47 @@ +/** + * @file Group.h + * + * @brief Concept check class for variable types with Group properties + * A Group concept extends a Manifold + * + * @date Nov 5, 2011 + * @author Alex Cunningham + */ + +#pragma once + +namespace gtsam { + +/** + * Concept check for general Group structure + */ +template +class GroupConcept { +private: + static void concept_check(const T& t) { + /** assignment */ + T t2 = t; + + /** compose with another object */ + T compose_ret = t.compose(t2); + + /** invert the object and yield a new one */ + T inverse_ret = t.inverse(); + + /** identity */ + T identity = T::identity(); + } +}; + +} // \namespace gtsam + +/** + * Macros for using the GroupConcept + * - An instantiation for use inside unit tests + * - A typedef for use inside generic algorithms + * + * NOTE: intentionally not in the gtsam namespace to allow for classes not in + * the gtsam namespace to be more easily enforced as testable + */ +#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::GroupConcept; +#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 61e7b6dfe..fb2146752 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -24,21 +24,9 @@ * concept checking function in class Lie will check whether or not * the function exists and throw compile-time errors. * - * Returns dimensionality of the tangent space - * inline size_t dim() const; - * - * Returns Exponential map update of T - * A default implementation of expmap(*this, lp) is available: - * expmap_default() - * T expmap(const Vector& v) const; - * - * expmap around identity + * Expmap around identity * static T Expmap(const Vector& v); * - * Returns Log map - * A default implementation of logmap(*this, lp) is available: - * logmap_default() - * Vector logmap(const T& lp) const; * * Logmap around identity * static Vector Logmap(const T& p); @@ -48,18 +36,13 @@ * between_default() * T between(const T& l2) const; * - * compose with another object - * T compose(const T& p) const; - * - * invert the object and yield a new one - * T inverse() const; - * */ #pragma once #include +#include namespace gtsam { @@ -101,32 +84,14 @@ namespace gtsam { */ size_t dim_ret = t.dim(); - /** - * Returns Exponential map update of T - * Default implementation calls global binary function - */ - T expmap_ret = t.expmap(gtsam::zero(dim_ret)); - /** expmap around identity */ T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - /** - * Returns Log map - * Default Implementation calls global binary function - */ - Vector logmap_ret = t.logmap(t2); - /** Logmap around identity */ Vector logmap_identity_ret = T::Logmap(t); /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ T between_ret = t.between(t2); - - /** compose with another object */ - T compose_ret = t.compose(t2); - - /** invert the object and yield a new one */ - T inverse_ret = t.inverse(); } }; @@ -156,6 +121,7 @@ namespace gtsam { * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula */ + /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? template T BCH(const T& X, const T& Y) { static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; @@ -185,13 +151,19 @@ namespace gtsam { } // namespace gtsam /** - * Macros for using the ManifoldConcept + * Macros for using the LieConcept * - An instantiation for use inside unit tests * - A typedef for use inside generic algorithms * * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -/// TODO: find better name for "INST" macro, something like "UNIT" or similar -#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::LieConcept; -#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::LieConcept _gtsam_LieConcept_##T; +#define GTSAM_CONCEPT_LIE_INST(T) \ + template class gtsam::ManifoldConcept; \ + template class gtsam::GroupConcept; \ + template class gtsam::LieConcept; + +#define GTSAM_CONCEPT_LIE_TYPE(T) \ + typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ + typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ + typedef gtsam::LieConcept _gtsam_LieConcept_##T; diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index a0f6cd6d6..51439c6f1 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -26,8 +26,8 @@ namespace gtsam { */ struct LieScalar { - /** default constructor - should be unnecessary */ - LieScalar() {} + /** default constructor */ + LieScalar() : d_(0.0) {} /** wrap a double */ LieScalar(double d) : d_(d) {} @@ -45,55 +45,51 @@ namespace gtsam { return fabs(expected.d_ - d_) <= tol; } - /** - * Returns dimensionality of the tangent space - * with member and static versions - */ + // Manifold requirements + + /** Returns dimensionality of the tangent space */ inline size_t dim() const { return 1; } inline static size_t Dim() { return 1; } - /** - * Returns Exponential map update of T - * Default implementation calls global binary function - */ - inline LieScalar expmap(const Vector& v) const { return LieScalar(d_ + v(0)); } + /** Update the LieScalar with a tangent space update */ + inline LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } - /** expmap around identity */ - static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); } - /** - * Returns Log map - * Default Implementation calls global binary function - */ - inline Vector logmap(const LieScalar& lp) const { - return Vector_(1, lp.d_ - d_); + // Group requirements + + /** identity */ + inline static LieScalar identity() { + return LieScalar(); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieScalar& p) { return Vector_(1, p.d_); } - - inline LieScalar between(const LieScalar& t2) const { return LieScalar(t2.value() - d_); } - /** compose with another object */ - inline LieScalar compose(const LieScalar& t2) const { return LieScalar(t2.value() + d_); } + inline LieScalar compose(const LieScalar& p) const { + return LieScalar(d_ + p.d_); + } + + /** between operation */ + inline LieScalar between(const LieScalar& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(1); + if(H2) *H2 = eye(1); + return LieScalar(l2.value() - value()); + } /** invert the object and yield a new one */ - inline LieScalar inverse() const { return LieScalar(-d_); } + inline LieScalar inverse() const { + return LieScalar(-1.0 * value()); + } - // Manifold requirements + // Lie functions - inline LieScalar retract(const Vector& v) const { return expmap(v); } + /** Expmap around identity */ + static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } - /** expmap around identity */ - inline static LieScalar Retract(const Vector& v) { return Expmap(v); } - - /** - * Returns inverse retraction - */ - inline Vector unretract(const LieScalar& t2) const { return logmap(t2); } - - /** Unretract around identity */ - inline static Vector Unretract(const LieScalar& t) { return Logmap(t); } + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); } private: double d_; diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index cf4e5b82b..5a1b3d580 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -57,31 +57,25 @@ struct LieVector : public Vector { return gtsam::equal(vector(), expected.vector(), tol); } - /** - * Returns dimensionality of the tangent space - */ + // Manifold requirements + + /** Returns dimensionality of the tangent space */ inline size_t dim() const { return this->size(); } - /** - * Returns Exponential map update of T - * Default implementation calls global binary function - */ - inline LieVector expmap(const Vector& v) const { return LieVector(vector() + v); } + /** Update the LieVector with a tangent space update */ + inline LieVector retract(const Vector& v) const { return LieVector(vector() + v); } - /** expmap around identity */ - static inline LieVector Expmap(const Vector& v) { return LieVector(v); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); } - /** - * Returns Log map - * Default Implementation calls global binary function - */ - inline Vector logmap(const LieVector& lp) const { - return lp.vector() - vector(); + // Group requirements + + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieVector identity() { + throw std::runtime_error("LieVector::identity(): Don't use this function"); + return LieVector(); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieVector& p) { return p; } - /** compose with another object */ inline LieVector compose(const LieVector& p) const { return LieVector(vector() + p); @@ -101,19 +95,13 @@ struct LieVector : public Vector { return LieVector(-1.0 * vector()); } - // Manifold requirements + // Lie functions - inline LieVector retract(const Vector& v) const { return expmap(v); } + /** Expmap around identity */ + static inline LieVector Expmap(const Vector& v) { return LieVector(v); } - /** expmap around identity */ - inline static LieVector Retract(const Vector& v) { return Expmap(v); } + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieVector& p) { return p; } - /** - * Returns inverse retraction - */ - inline Vector unretract(const LieVector& t2) const { return logmap(t2); } - - /** Unretract around identity */ - inline static Vector Unretract(const LieVector& t) { return Logmap(t); } }; } // \namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 75a922efb..87506f31f 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -26,19 +26,11 @@ * Returns Retraction update of T * T retract(const Vector& v) const; * - * Retract around identity - * static T Retract(const Vector& v); - * * Returns inverse retraction operation - * A default implementation of unretract(*this, lp) is available: - * Vector unretract(const T& lp) const; - * - * Unretract around identity - * static Vector Unretract(const T& p); + * Vector localCoordinates(const T& lp) const; * */ - #pragma once #include @@ -74,16 +66,10 @@ namespace gtsam { */ T retract_ret = t.retract(gtsam::zero(dim_ret)); - /** expmap around identity */ - T retract_identity_ret = T::Retract(gtsam::zero(dim_ret)); - /** - * Returns inverse retraction + * Returns local coordinates of another object */ - Vector unretract_ret = t.unretract(t2); - - /** Unretract around identity */ - Vector unretract_identity_ret = T::Unretract(t); + Vector localCoords_ret = t.localCoordinates(t2); } }; @@ -97,6 +83,5 @@ namespace gtsam { * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -/// TODO: find better name for "INST" macro, something like "UNIT" or similar #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept; #define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index 41e031cf7..ecdb974d5 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -36,13 +36,6 @@ namespace testing { template T compose(const T& t1, const T& t2) { return t1.compose(t2); } - /** expmap and logmap */ - template - Vector logmap(const T& t1, const T& t2) { return t1.logmap(t2); } - - template - T expmap(const T& t1, const Vector& t2) { return t1.expmap(t2); } - /** unary functions */ template T inverse(const T& t) { return t.inverse(); } diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 57a5b0f50..d18db38be 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -97,8 +97,8 @@ namespace gtsam { Vector d = zero(n); Matrix H = zeros(m,n); for (size_t j=0;j class GeneralCameraT { - private: - Camera calibrated_; // Calibrated camera - Calibration calibration_; // Calibration +private: + Camera calibrated_; // Calibrated camera + Calibration calibration_; // Calibration - public: - GeneralCameraT(){} - GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {} - GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {} - GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {} - GeneralCameraT(const Pose3& pose) : calibrated_(pose) {} - GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {} +public: + GeneralCameraT(){} + GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {} + GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {} + GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {} + GeneralCameraT(const Pose3& pose) : calibrated_(pose) {} + GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {} - // Vector Initialization - GeneralCameraT(const Vector &v) : - calibrated_(sub(v, 0, Camera::Dim())), - calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {} + // Vector Initialization + GeneralCameraT(const Vector &v) : + calibrated_(sub(v, 0, Camera::Dim())), + calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {} - inline const Pose3& pose() const { return calibrated_.pose(); } - inline const Camera& calibrated() const { return calibrated_; } - inline const Calibration& calibration() const { return calibration_; } + inline const Pose3& pose() const { return calibrated_.pose(); } + inline const Camera& calibrated() const { return calibrated_; } + inline const Calibration& calibration() const { return calibration_; } - std::pair projectSafe( - const Point3& P, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + std::pair projectSafe( + const Point3& P, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { - Point3 cameraPoint = calibrated_.pose().transform_to(P); - return std::pair( - project(P,H1,H2), - cameraPoint.z() > 0); + Point3 cameraPoint = calibrated_.pose().transform_to(P); + return std::pair( + project(P,H1,H2), + cameraPoint.z() > 0); + } + + Point3 backproject(const Point2& projection, const double scale) const { + Point2 intrinsic = calibration_.calibrate(projection); + Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); + return calibrated_.pose().transform_from(cameraPoint); + } + + /** + * project function that does not merge the Jacobians of calibration and pose + */ + Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const { + Matrix tmp; + Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt); + Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp); + H1_pose = tmp * H1_pose; + H2_pt = tmp * H2_pt; + return projection; + } + + /** + * project a 3d point to the camera + * P is point in camera coordinate + * H1 is respect to pose + calibration + * H2 is respect to landmark + */ + Point2 project(const Point3& P, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + + if (!H1 && !H2) { + Point2 intrinsic = calibrated_.project(P); + return calibration_.uncalibrate(intrinsic) ; } - Point3 backproject(const Point2& projection, const double scale) const { - Point2 intrinsic = calibration_.calibrate(projection); - Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); - return calibrated_.pose().transform_from(cameraPoint); - } + Matrix H1_k, H1_pose, H2_pt; + Point2 projection = project(P, H1_pose, H1_k, H2_pt); + if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k); + if ( H2 ) *H2 = H2_pt; - /** - * project function that does not merge the Jacobians of calibration and pose - */ - Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const { - Matrix tmp; - Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt); - Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp); - H1_pose = tmp * H1_pose; - H2_pt = tmp * H2_pt; - return projection; - } + return projection; + } - /** - * project a 3d point to the camera - * P is point in camera coordinate - * H1 is respect to pose + calibration - * H2 is respect to landmark - */ - Point2 project(const Point3& P, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + // dump functions for compilation for now + bool equals (const GeneralCameraT &camera, double tol = 1e-9) const { + return calibrated_.equals(camera.calibrated_, tol) && + calibration_.equals(camera.calibration_, tol) ; + } - if (!H1 && !H2) { - Point2 intrinsic = calibrated_.project(P); - return calibration_.uncalibrate(intrinsic) ; - } + void print(const std::string& s = "") const { + calibrated_.pose().print(s + ".camera.") ; + calibration_.print(s + ".calibration.") ; + } - Matrix H1_k, H1_pose, H2_pt; - Point2 projection = project(P, H1_pose, H1_k, H2_pt); - if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k); - if ( H2 ) *H2 = H2_pt; + GeneralCameraT retract(const Vector &v) const { + Vector v1 = sub(v,0,Camera::Dim()); + Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); + return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2)); + } - return projection; - } + Vector localCoordinates(const GeneralCameraT &C) const { + const Vector v1(calibrated().localCoordinates(C.calibrated())), + v2(calibration().localCoordinates(C.calibration())); + return concatVectors(2,&v1,&v2) ; + } - // dump functions for compilation for now - bool equals (const GeneralCameraT &camera, double tol = 1e-9) const { - return calibrated_.equals(camera.calibrated_, tol) && - calibration_.equals(camera.calibration_, tol) ; - } + inline GeneralCameraT compose(const Pose3 &p) const { + return GeneralCameraT( pose().compose(p), calibration_ ) ; + } - void print(const std::string& s = "") const { - calibrated_.pose().print(s + ".camera.") ; - calibration_.print(s + ".calibration.") ; - } + Matrix D2d_camera(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; + Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - GeneralCameraT retract(const Vector &v) const { - Vector v1 = sub(v,0,Camera::Dim()); - Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); - return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2)); - } + const int n1 = calibrated_.dim() ; + const int n2 = calibration_.dim() ; + Matrix D(2,n1+n2) ; - Vector unretract(const GeneralCameraT &C) const { - const Vector v1(calibrated().unretract(C.calibrated())), - v2(calibration().unretract(C.calibration())); - return concatVectors(2,&v1,&v2) ; - } + sub(D,0,2,0,n1) = D_2d_pose ; + sub(D,0,2,n1,n1+n2) = D_2d_calibration ; + return D; + } - static GeneralCameraT Retract(const Vector& v) { - return GeneralCameraT( - Camera::Retract(sub(v,0,Camera::Dim())), - Calibration::Retract(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim())) - ); - } + Matrix D2d_3d(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + return D_2d_intrinsic * D_intrinsic_3d; + } - static Vector Unretract(const GeneralCameraT& p) { - const Vector v1(Camera::Unretract(p.calibrated())), - v2(Calibration::Unretract(p.calibration())); - return concatVectors(2,&v1,&v2); + Matrix D2d_camera_3d(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; + Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - } + Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); + Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d; - inline GeneralCameraT compose(const Pose3 &p) const { - return GeneralCameraT( pose().compose(p), calibration_ ) ; - } + const int n1 = calibrated_.dim() ; + const int n2 = calibration_.dim() ; - Matrix D2d_camera(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; - Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); + Matrix D(2,n1+n2+3) ; - const int n1 = calibrated_.dim() ; - const int n2 = calibration_.dim() ; - Matrix D(2,n1+n2) ; + sub(D,0,2,0,n1) = D_2d_pose ; + sub(D,0,2,n1,n1+n2) = D_2d_calibration ; + sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ; + return D; + } - sub(D,0,2,0,n1) = D_2d_pose ; - sub(D,0,2,n1,n1+n2) = D_2d_calibration ; - return D; - } - - Matrix D2d_3d(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - return D_2d_intrinsic * D_intrinsic_3d; - } - - Matrix D2d_camera_3d(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; - Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - - Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); - Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d; - - const int n1 = calibrated_.dim() ; - const int n2 = calibration_.dim() ; - - Matrix D(2,n1+n2+3) ; - - sub(D,0,2,0,n1) = D_2d_pose ; - sub(D,0,2,n1,n1+n2) = D_2d_calibration ; - sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ; - return D; - } - - //inline size_t dim() { return Camera::dim() + Calibration::dim() ; } - inline size_t dim() const { return calibrated().dim() + calibration().dim() ; } - static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; } + //inline size_t dim() { return Camera::dim() + Calibration::dim() ; } + inline size_t dim() const { return calibrated().dim() + calibration().dim() ; } + static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; } private: - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(calibrated_); - ar & BOOST_SERIALIZATION_NVP(calibration_); - } + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(calibrated_); + ar & BOOST_SERIALIZATION_NVP(calibration_); + } - }; +}; typedef GeneralCameraT Cal3BundlerCamera; typedef GeneralCameraT Cal3DS2Camera; diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index ca83dfa8a..0834cdcda 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -22,22 +22,22 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Point2); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Point2); - /* ************************************************************************* */ - void Point2::print(const string& s) const { - cout << s << "(" << x_ << ", " << y_ << ")" << endl; - } +/* ************************************************************************* */ +void Point2::print(const string& s) const { + cout << s << "(" << x_ << ", " << y_ << ")" << endl; +} - /* ************************************************************************* */ - bool Point2::equals(const Point2& q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); - } +/* ************************************************************************* */ +bool Point2::equals(const Point2& q, double tol) const { + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); +} - /* ************************************************************************* */ - double Point2::norm() const { - return sqrt(x_*x_ + y_*y_); - } +/* ************************************************************************* */ +double Point2::norm() const { + return sqrt(x_*x_ + y_*y_); +} } // namespace gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 79607b276..5b335f564 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -23,127 +23,122 @@ namespace gtsam { - /** - * A 2D point - * Complies with the Testable Concept - * Functional, so no set functions: once created, a point is constant. - * @ingroup geometry - */ - class Point2 { - public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 2; - private: - double x_, y_; - - public: - Point2(): x_(0), y_(0) {} - Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {} - Point2(double x, double y): x_(x), y_(y) {} - Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); } +/** + * A 2D point + * Complies with the Testable Concept + * Functional, so no set functions: once created, a point is constant. + * @ingroup geometry + */ +class Point2 { +public: + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 2; +private: + double x_, y_; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } +public: + Point2(): x_(0), y_(0) {} + Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {} + Point2(double x, double y): x_(x), y_(y) {} + Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); } - /** print with optional string */ - void print(const std::string& s = "") const; + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } - /** equals with an tolerance, prints out message if unequal*/ - bool equals(const Point2& q, double tol = 1e-9) const; + /** print with optional string */ + void print(const std::string& s = "") const; - /** Lie requirements */ + /** equals with an tolerance, prints out message if unequal*/ + bool equals(const Point2& q, double tol = 1e-9) const; - /** Size of the tangent space of the Lie type */ - inline size_t dim() const { return dimension; } + // Group requirements - /** "Compose", just adds the coordinates of two points. With optional derivatives */ - inline Point2 compose(const Point2& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(2); - if(H2) *H2 = eye(2); - return *this + p2; - } + /** "Compose", just adds the coordinates of two points. With optional derivatives */ + inline Point2 compose(const Point2& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(2); + if(H2) *H2 = eye(2); + return *this + p2; + } - /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ - inline Point2 inverse() const { return Point2(-x_, -y_); } + /** identity */ + inline static Point2 identity() { + return Point2(); + } - /** Binary expmap - just adds the points */ - inline Point2 expmap(const Vector& v) const { return *this + Point2(v); } + /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ + inline Point2 inverse() const { return Point2(-x_, -y_); } - /** Binary Logmap - just subtracts the points */ - inline Vector logmap(const Point2& p2) const { return Logmap(between(p2));} + // Manifold requirements - /** Exponential map around identity - just create a Point2 from a vector */ - static inline Point2 Expmap(const Vector& v) { return Point2(v); } + /** Size of the tangent space */ + inline size_t dim() const { return dimension; } - /** Log map around identity - just return the Point2 as a vector */ - static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } + /** Updates a with tangent space delta */ + inline Point2 retract(const Vector& v) const { return *this + Point2(v); } - // Manifold requirements + /// Local coordinates of manifold neighborhood around current value + inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } - inline Point2 retract(const Vector& v) const { return expmap(v); } + /** Lie requirements */ - /** expmap around identity */ - inline static Point2 Retract(const Vector& v) { return Expmap(v); } + /** Exponential map around identity - just create a Point2 from a vector */ + static inline Point2 Expmap(const Vector& v) { return Point2(v); } - /** - * Returns inverse retraction - */ - inline Vector unretract(const Point2& t2) const { return logmap(t2); } + /** Log map around identity - just return the Point2 as a vector */ + static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } - /** Unretract around identity */ - inline static Vector Unretract(const Point2& t) { return Logmap(t); } - /** "Between", subtracts point coordinates */ - inline Point2 between(const Point2& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(2); - if(H2) *H2 = eye(2); - return p2 - (*this); - } + /** "Between", subtracts point coordinates */ + inline Point2 between(const Point2& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(2); + if(H2) *H2 = eye(2); + return p2 - (*this); + } - /** get functions for x, y */ - double x() const {return x_;} - double y() const {return y_;} + /** get functions for x, y */ + double x() const {return x_;} + double y() const {return y_;} - /** return vectorized form (column-wise) */ - Vector vector() const { return Vector_(2, x_, y_); } + /** return vectorized form (column-wise) */ + Vector vector() const { return Vector_(2, x_, y_); } - /** operators */ - inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} - inline void operator *= (double s) {x_*=s;y_*=s;} - inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} - inline Point2 operator- () const {return Point2(-x_,-y_);} - inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} - inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} - inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} + /** operators */ + inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} + inline void operator *= (double s) {x_*=s;y_*=s;} + inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} + inline Point2 operator- () const {return Point2(-x_,-y_);} + inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} + inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} + inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} + inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - /** norm of point */ - double norm() const; + /** norm of point */ + double norm() const; - /** creates a unit vector */ - Point2 unit() const { return *this/norm(); } + /** creates a unit vector */ + Point2 unit() const { return *this/norm(); } - /** distance between two points */ - inline double dist(const Point2& p2) const { - return (p2 - *this).norm(); - } + /** distance between two points */ + inline double dist(const Point2& p2) const { + return (p2 - *this).norm(); + } - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(x_); - ar & BOOST_SERIALIZATION_NVP(y_); - } - }; +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(x_); + ar & BOOST_SERIALIZATION_NVP(y_); + } +}; - /** multiply with scalar */ - inline Point2 operator*(double s, const Point2& p) {return p*s;} +/** multiply with scalar */ +inline Point2 operator*(double s, const Point2& p) {return p*s;} } diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 9573cc187..c9f89d83c 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -20,71 +20,71 @@ namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Point3); +/** Explicit instantiation of base class to export members */ +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); - } +/* ************************************************************************* */ +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); +} - /* ************************************************************************* */ +/* ************************************************************************* */ - void Point3::print(const std::string& s) const { - std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; - } +void Point3::print(const std::string& s) const { + std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; +} - /* ************************************************************************* */ +/* ************************************************************************* */ - bool Point3::operator== (const Point3& q) const { - return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; - } +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- (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); - } - /* ************************************************************************* */ - 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); - 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_ ); - } - /* ************************************************************************* */ - double Point3::dot(const Point3 &q) const { - return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); - } - /* ************************************************************************* */ - double Point3::norm() const { - return sqrt( x_*x_ + y_*y_ + z_*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); +} +/* ************************************************************************* */ +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); + 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_ ); +} +/* ************************************************************************* */ +double Point3::dot(const Point3 &q) const { + return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); +} +/* ************************************************************************* */ +double Point3::norm() const { + return sqrt( x_*x_ + y_*y_ + z_*z_ ); +} +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index d5740f2f2..2f8c9f8cc 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -60,6 +60,11 @@ namespace gtsam { /** return DOF, dimensionality of tangent space */ inline size_t dim() const { return dimension; } + /** identity */ + inline static Point3 identity() { + return Point3(); + } + /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } @@ -78,24 +83,14 @@ namespace gtsam { /** Log map at identity - return the x,y,z of this point */ static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } - /** default implementations of binary functions */ - inline Point3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); } - inline Vector logmap(const Point3& p2) const { return gtsam::logmap_default(*this, p2);} - // Manifold requirements - inline Point3 retract(const Vector& v) const { return expmap(v); } - - /** expmap around identity */ - inline static Point3 Retract(const Vector& v) { return Expmap(v); } + inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } /** * Returns inverse retraction */ - inline Vector unretract(const Point3& t2) const { return logmap(t2); } - - /** Unretract around identity */ - inline static Vector Unretract(const Point3& t) { return Logmap(t); } + inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } /** Between using the default implementation */ inline Point3 between(const Point3& p2, diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 567e0f7f4..a15929c67 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -25,287 +25,278 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Pose2); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Pose2); - /** instantiate concept checks */ - GTSAM_CONCEPT_POSE_INST(Pose2); +/** instantiate concept checks */ +GTSAM_CONCEPT_POSE_INST(Pose2); - static const Matrix I3 = eye(3), Z12 = zeros(1,2); - static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); +static const Matrix I3 = eye(3), Z12 = zeros(1,2); +static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); - /* ************************************************************************* */ - Matrix Pose2::matrix() const { - Matrix R = r_.matrix(); - R = stack(2, &R, &Z12); - Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0); - return collect(2, &R, &T); - } +/* ************************************************************************* */ +Matrix Pose2::matrix() const { + Matrix R = r_.matrix(); + R = stack(2, &R, &Z12); + Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0); + return collect(2, &R, &T); +} - /* ************************************************************************* */ - void Pose2::print(const string& s) const { - cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; - } +/* ************************************************************************* */ +void Pose2::print(const string& s) const { + cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; +} - /* ************************************************************************* */ - bool Pose2::equals(const Pose2& q, double tol) const { - return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); - } +/* ************************************************************************* */ +bool Pose2::equals(const Pose2& q, double tol) const { + return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); +} - /* ************************************************************************* */ - Pose2 Pose2::Expmap(const Vector& xi) { - assert(xi.size() == 3); - Point2 v(xi(0),xi(1)); - 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; - return Pose2(R, t); - } +/* ************************************************************************* */ +Pose2 Pose2::Expmap(const Vector& xi) { + assert(xi.size() == 3); + Point2 v(xi(0),xi(1)); + 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; + return Pose2(R, t); } +} - /* ************************************************************************* */ - Vector Pose2::Logmap(const Pose2& p) { - const Rot2& R = p.r(); - const Point2& t = p.t(); - double w = R.theta(); - if (std::abs(w) < 1e-10) - return Vector_(3, t.x(), t.y(), w); - else { - double c_1 = R.c()-1.0, s = R.s(); - double det = c_1*c_1 + s*s; - Point2 p = R_PI_2 * (R.unrotate(t) - t); - Point2 v = (w/det) * p; - return Vector_(3, v.x(), v.y(), w); - } - } +/* ************************************************************************* */ +Vector Pose2::Logmap(const Pose2& p) { + const Rot2& R = p.r(); + const Point2& t = p.t(); + double w = R.theta(); + if (std::abs(w) < 1e-10) + return Vector_(3, t.x(), t.y(), w); + else { + double c_1 = R.c()-1.0, s = R.s(); + double det = c_1*c_1 + s*s; + Point2 p = R_PI_2 * (R.unrotate(t) - t); + Point2 v = (w/det) * p; + return Vector_(3, v.x(), v.y(), w); + } +} - /* ************************************************************************* */ +/* ************************************************************************* */ +Pose2 Pose2::retract(const Vector& v) const { #ifdef SLOW_BUT_CORRECT_EXPMAP - /* ************************************************************************* */ - // Changes default to use the full verions of expmap/logmap - /* ************************************************************************* */ - Pose2 Pose2::Retract(const Vector& xi) { - return Expmap(xi); - } - - /* ************************************************************************* */ - Vector Pose2::Unretract(const Pose2& p) { - return Logmap(p); - } - + return compose(Expmap(v)); #else - - /* ************************************************************************* */ - Pose2 Pose2::Retract(const Vector& v) { - assert(v.size() == 3); - return Pose2(v[0], v[1], v[2]); - } - - /* ************************************************************************* */ - Vector Pose2::Unretract(const Pose2& p) { - return Vector_(3, p.x(), p.y(), p.theta()); - } - + assert(v.size() == 3); + return compose(Pose2(v[0], v[1], v[2])); #endif +} - /* ************************************************************************* */ - // Calculate Adjoint map - // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) - Matrix Pose2::AdjointMap() const { - double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); - return Matrix_(3,3, - c, -s, y, - s, c, -x, - 0.0, 0.0, 1.0 - ); - } +/* ************************************************************************* */ +Vector Pose2::localCoordinates(const Pose2& p2) const { +#ifdef SLOW_BUT_CORRECT_EXPMAP + return Logmap(between(p2)); +#else + Pose2 r = between(p2); + return Vector_(3, r.x(), r.y(), r.theta()); +#endif +} - /* ************************************************************************* */ - Pose2 Pose2::inverse(boost::optional H1) const { - if (H1) *H1 = -AdjointMap(); - return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); - } +/* ************************************************************************* */ +// Calculate Adjoint map +// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) +Matrix Pose2::AdjointMap() const { + double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); + return Matrix_(3,3, + c, -s, y, + s, c, -x, + 0.0, 0.0, 1.0 + ); +} - /* ************************************************************************* */ - // see doc/math.lyx, SE(2) section - Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = point - t_; - Point2 q = r_.unrotate(d); - if (!H1 && !H2) return q; - if (H1) *H1 = Matrix_(2, 3, - -1.0, 0.0, q.y(), - 0.0, -1.0, -q.x()); - if (H2) *H2 = r_.transpose(); - return q; +/* ************************************************************************* */ +Pose2 Pose2::inverse(boost::optional H1) const { + if (H1) *H1 = -AdjointMap(); + return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_to(const Point2& point, + boost::optional H1, boost::optional H2) const { + Point2 d = point - t_; + Point2 q = r_.unrotate(d); + if (!H1 && !H2) return q; + if (H1) *H1 = Matrix_(2, 3, + -1.0, 0.0, q.y(), + 0.0, -1.0, -q.x()); + if (H2) *H2 = r_.transpose(); + return q; +} + +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, + boost::optional H2) const { + // TODO: inline and reuse? + if(H1) *H1 = p2.inverse().AdjointMap(); + if(H2) *H2 = I3; + return (*this)*p2; +} + +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_from(const Point2& p, + boost::optional H1, boost::optional H2) const { + const Point2 q = r_ * p; + if (H1 || H2) { + const Matrix R = r_.matrix(); + const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x()); + if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] + if (H2) *H2 = R; // R + } + return q + t_; +} + +/* ************************************************************************* */ +Pose2 Pose2::between(const Pose2& p2, boost::optional H1, + boost::optional H2) const { + // get cosines and sines from rotation matrices + const Rot2& R1 = r_, R2 = p2.r(); + double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); + + // Assert that R1 and R2 are normalized + assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); + + // Calculate delta rotation = between(R1,R2) + double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; + Rot2 R(Rot2::atan2(s,c)); // normalizes + + // Calculate delta translation = unrotate(R1, dt); + Point2 dt = p2.t() - t_; + double x = dt.x(), y = dt.y(); + Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); + + // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above + if (H1) { + double dt1 = -s2 * x + c2 * y; + double dt2 = -c2 * x - s2 * y; + *H1 = Matrix_(3,3, + -c, -s, dt1, + s, -c, dt2, + 0.0, 0.0,-1.0); + } + if (H2) *H2 = I3; + + return Pose2(R,t); +} + +/* ************************************************************************* */ +Rot2 Pose2::bearing(const Point2& point, + boost::optional H1, boost::optional H2) const { + Point2 d = transform_to(point, H1, H2); + if (!H1 && !H2) return Rot2::relativeBearing(d); + Matrix D_result_d; + Rot2 result = Rot2::relativeBearing(d, D_result_d); + if (H1) *H1 = D_result_d * (*H1); + if (H2) *H2 = D_result_d * (*H2); + return result; +} + +/* ************************************************************************* */ +Rot2 Pose2::bearing(const Pose2& point, + boost::optional H1, boost::optional H2) const { + Rot2 result = bearing(point.t(), H1, H2); + if (H2) { + Matrix H2_ = *H2 * point.r().matrix(); + *H2 = zeros(1, 3); + insertSub(*H2, H2_, 0, 0); + } + return result; +} + +/* ************************************************************************* */ +double Pose2::range(const Point2& point, + boost::optional H1, boost::optional H2) const { + if (!H1 && !H2) return transform_to(point).norm(); + Point2 d = transform_to(point, H1, H2); + double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2); + Matrix D_result_d; + if(std::abs(r) > 1e-10) + D_result_d = Matrix_(1, 2, x / r, y / r); + else { + D_result_d = Matrix_(1,2, 1.0, 1.0); + } + if (H1) *H1 = D_result_d * (*H1); + if (H2) *H2 = D_result_d * (*H2); + return r; +} + +/* ************************************************************************* */ +double Pose2::range(const Pose2& point, + boost::optional H1, + boost::optional H2) const { + double r = range(point.t(), H1, H2); + if (H2) { + // NOTE: expmap changes the orientation of expmap direction, + // so we must rotate the jacobian + Matrix H2_ = *H2 * point.r().matrix(); + *H2 = zeros(1, 3); + insertSub(*H2, H2_, 0, 0); + } + return r; +} + +/* ************************************************************************* + * New explanation, from scan.ml + * It finds the angle using a linear method: + * q = Pose2::transform_from(p) = t + R*p + * We need to remove the centroids from the data to find the rotation + * using dp=[dpx;dpy] and q=[dqx;dqy] we have + * |dqx| |c -s| |dpx| |dpx -dpy| |c| + * | | = | | * | | = | | * | | = H_i*cs + * |dqy| |s c| |dpy| |dpy dpx| |s| + * where the Hi are the 2*2 matrices. Then we will minimize the criterion + * J = \sum_i norm(q_i - H_i * cs) + * Taking the derivative with respect to cs and setting to zero we have + * cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i) + * The hessian is diagonal and just divides by a constant, but this + * normalization constant is irrelevant, since we take atan2. + * i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy) + * The translation is then found from the centroids + * as they also satisfy cq = t + R*cp, hence t = cq - R*cp + */ + +boost::optional align(const vector& pairs) { + + size_t n = pairs.size(); + if (n<2) return boost::none; // we need at least two pairs + + // calculate centroids + Point2 cp,cq; + BOOST_FOREACH(const Point2Pair& pair, pairs) { + cp += pair.first; + cq += pair.second; + } + double f = 1.0/n; + cp *= f; cq *= f; + + // calculate cos and sin + double c=0,s=0; + BOOST_FOREACH(const Point2Pair& pair, pairs) { + Point2 dq = pair.first - cp; + Point2 dp = pair.second - cq; + c += dp.x() * dq.x() + dp.y() * dq.y(); + s += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-( } - /* ************************************************************************* */ - // see doc/math.lyx, SE(2) section - Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // TODO: inline and reuse? - if(H1) *H1 = p2.inverse().AdjointMap(); - if(H2) *H2 = I3; - return (*this)*p2; - } + // calculate angle and translation + double theta = atan2(s,c); + Rot2 R = Rot2::fromAngle(theta); + Point2 t = cq - R*cp; + return Pose2(R, t); +} - /* ************************************************************************* */ - // see doc/math.lyx, SE(2) section - Point2 Pose2::transform_from(const Point2& p, - boost::optional H1, boost::optional H2) const { - const Point2 q = r_ * p; - if (H1 || H2) { - const Matrix R = r_.matrix(); - const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x()); - if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] - if (H2) *H2 = R; // R - } - return q + t_; - } - - /* ************************************************************************* */ - Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above - if (H1) { - double dt1 = -s2 * x + c2 * y; - double dt2 = -c2 * x - s2 * y; - *H1 = Matrix_(3,3, - -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0,-1.0); - } - if (H2) *H2 = I3; - - return Pose2(R,t); - } - - /* ************************************************************************* */ - Rot2 Pose2::bearing(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = transform_to(point, H1, H2); - if (!H1 && !H2) return Rot2::relativeBearing(d); - Matrix D_result_d; - Rot2 result = Rot2::relativeBearing(d, D_result_d); - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); - return result; - } - - /* ************************************************************************* */ - Rot2 Pose2::bearing(const Pose2& point, - boost::optional H1, boost::optional H2) const { - Rot2 result = bearing(point.t(), H1, H2); - if (H2) { - Matrix H2_ = *H2 * point.r().matrix(); - *H2 = zeros(1, 3); - insertSub(*H2, H2_, 0, 0); - } - return result; - } - - /* ************************************************************************* */ - double Pose2::range(const Point2& point, - boost::optional H1, boost::optional H2) const { - if (!H1 && !H2) return transform_to(point).norm(); - Point2 d = transform_to(point, H1, H2); - double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2); - Matrix D_result_d; - if(std::abs(r) > 1e-10) - D_result_d = Matrix_(1, 2, x / r, y / r); - else { - D_result_d = Matrix_(1,2, 1.0, 1.0); - } - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); - return r; - } - - /* ************************************************************************* */ - double Pose2::range(const Pose2& point, - boost::optional H1, - boost::optional H2) const { - double r = range(point.t(), H1, H2); - if (H2) { - // NOTE: expmap changes the orientation of expmap direction, - // so we must rotate the jacobian - Matrix H2_ = *H2 * point.r().matrix(); - *H2 = zeros(1, 3); - insertSub(*H2, H2_, 0, 0); - } - return r; - } - - /* ************************************************************************* - * New explanation, from scan.ml - * It finds the angle using a linear method: - * q = Pose2::transform_from(p) = t + R*p - * We need to remove the centroids from the data to find the rotation - * using dp=[dpx;dpy] and q=[dqx;dqy] we have - * |dqx| |c -s| |dpx| |dpx -dpy| |c| - * | | = | | * | | = | | * | | = H_i*cs - * |dqy| |s c| |dpy| |dpy dpx| |s| - * where the Hi are the 2*2 matrices. Then we will minimize the criterion - * J = \sum_i norm(q_i - H_i * cs) - * Taking the derivative with respect to cs and setting to zero we have - * cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i) - * The hessian is diagonal and just divides by a constant, but this - * normalization constant is irrelevant, since we take atan2. - * i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy) - * The translation is then found from the centroids - * as they also satisfy cq = t + R*cp, hence t = cq - R*cp - */ - - boost::optional align(const vector& pairs) { - - size_t n = pairs.size(); - if (n<2) return boost::none; // we need at least two pairs - - // calculate centroids - Point2 cp,cq; - BOOST_FOREACH(const Point2Pair& pair, pairs) { - cp += pair.first; - cq += pair.second; - } - double f = 1.0/n; - cp *= f; cq *= f; - - // calculate cos and sin - double c=0,s=0; - BOOST_FOREACH(const Point2Pair& pair, pairs) { - Point2 dq = pair.first - cp; - Point2 dp = pair.second - cq; - c += dp.x() * dq.x() + dp.y() * dq.y(); - s += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-( - } - - // calculate angle and translation - double theta = atan2(s,c); - Rot2 R = Rot2::fromAngle(theta); - Point2 t = cq - R*cp; - return Pose2(R, t); - } - - /* ************************************************************************* */ +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 41fa865d9..06a4ad54b 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -27,148 +27,139 @@ namespace gtsam { - /** - * A 2D pose (Point2,Rot2) - * @ingroup geometry - */ - class Pose2 { +/** + * A 2D pose (Point2,Rot2) + * @ingroup geometry + */ +class Pose2 { - public: - static const size_t dimension = 3; +public: + static const size_t dimension = 3; - /** Pose Concept requirements */ - typedef Rot2 Rotation; - typedef Point2 Translation; + /** Pose Concept requirements */ + typedef Rot2 Rotation; + typedef Point2 Translation; - private: - Rot2 r_; - Point2 t_; +private: + Rot2 r_; + Point2 t_; - public: +public: - /** default constructor = origin */ - Pose2() {} // default is origin + /** default constructor = origin */ + Pose2() {} // default is origin - /** copy constructor */ - Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} + /** copy constructor */ + Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} - /** - * construct from (x,y,theta) - * @param x x coordinate - * @param y y coordinate - * @param theta angle with positive X-axis - */ - Pose2(double x, double y, double theta) : - r_(Rot2::fromAngle(theta)), t_(x, y) { - } + /** + * construct from (x,y,theta) + * @param x x coordinate + * @param y y coordinate + * @param theta angle with positive X-axis + */ + Pose2(double x, double y, double theta) : + r_(Rot2::fromAngle(theta)), t_(x, y) { + } - /** construct from rotation and translation */ - Pose2(double theta, const Point2& t) : - r_(Rot2::fromAngle(theta)), t_(t) { - } - Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} + /** construct from rotation and translation */ + Pose2(double theta, const Point2& t) : + r_(Rot2::fromAngle(theta)), t_(t) { + } + Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} - /** Constructor from 3*3 matrix */ - Pose2(const Matrix &T) : - r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { - assert(T.rows() == 3 && T.cols() == 3); - } + /** Constructor from 3*3 matrix */ + Pose2(const Matrix &T) : + r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { + assert(T.rows() == 3 && T.cols() == 3); + } - /** Construct from canonical coordinates (Lie algebra) */ - Pose2(const Vector& v) { - *this = Expmap(v); - } + /** Construct from canonical coordinates (Lie algebra) */ + Pose2(const Vector& v) { + *this = Expmap(v); + } - /** print with optional string */ - void print(const std::string& s = "") const; + /** print with optional string */ + void print(const std::string& s = "") const; - /** assert equality up to a tolerance */ - bool equals(const Pose2& pose, double tol = 1e-9) const; + /** assert equality up to a tolerance */ + bool equals(const Pose2& pose, double tol = 1e-9) const; - /** compose syntactic sugar */ - inline Pose2 operator*(const Pose2& p2) const { - return Pose2(r_*p2.r(), t_ + r_*p2.t()); - } + /** compose syntactic sugar */ + inline Pose2 operator*(const Pose2& p2) const { + return Pose2(r_*p2.r(), t_ + r_*p2.t()); + } - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } - /** Lie requirements */ + /** Lie requirements */ - /** return DOF, dimensionality of tangent space = 3 */ - inline size_t dim() const { return dimension; } + /** return DOF, dimensionality of tangent space = 3 */ + inline size_t dim() const { return dimension; } - /** - * inverse transformation with derivatives - */ - Pose2 inverse(boost::optional H1=boost::none) const; + /** + * inverse transformation with derivatives + */ + Pose2 inverse(boost::optional H1=boost::none) const; - /** - * compose this transformation onto another (first *this and then p2) - * With optional derivatives - */ - Pose2 compose(const Pose2& p2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + /** + * compose this transformation onto another (first *this and then p2) + * With optional derivatives + */ + Pose2 compose(const Pose2& p2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(compose(p2))); - } + /// MATLAB version returns shared pointer + boost::shared_ptr compose_(const Pose2& p2) { + return boost::shared_ptr(new Pose2(compose(p2))); + } - /** syntactic sugar for transform_from */ - inline Point2 operator*(const Point2& point) const { return transform_from(point);} + /** syntactic sugar for transform_from */ + inline Point2 operator*(const Point2& point) const { return transform_from(point);} - /** - * Retraction from se(2) to SE(2) - */ - static Pose2 Retract(const Vector& v); + /** identity */ + inline static Pose2 identity() { + return Pose2(); + } - /** - * Inverse of retraction, from SE(2) to se(2) - */ - static Vector Unretract(const Pose2& p); + /** Real versions of Expmap/Logmap */ + static Pose2 Expmap(const Vector& xi); + static Vector Logmap(const Pose2& p); - /** Real versions of Expmap/Logmap */ - static Pose2 Expmap(const Vector& xi); - static Vector Logmap(const Pose2& p); + /** default implementations of binary functions */ + Pose2 retract(const Vector& v) const; + Vector localCoordinates(const Pose2& p2) const; - /** default implementations of binary functions */ - inline Pose2 retract(const Vector& v) const { return compose(Retract(v)); } - inline Vector unretract(const Pose2& p2) const { return Unretract(between(p2));} + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose2 between(const Pose2& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** non-approximated versions of expmap/logmap */ - inline Pose2 expmap(const Vector& v) const { return compose(Expmap(v)); } - inline Vector logmap(const Pose2& p2) const { return Logmap(between(p2));} + /// MATLAB version returns shared pointer + boost::shared_ptr between_(const Pose2& p2) { + return boost::shared_ptr(new Pose2(between(p2))); + } - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /** return transformation matrix */ + Matrix matrix() const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(between(p2))); - } + /** + * Return point coordinates in pose coordinate frame + */ + Point2 transform_to(const Point2& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** return transformation matrix */ - Matrix matrix() const; - - /** - * Return point coordinates in pose coordinate frame - */ - Point2 transform_to(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - /** - * Return point coordinates in global frame - */ - Point2 transform_from(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /** + * Return point coordinates in global frame + */ + Point2 transform_from(const Point2& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; /** * Calculate bearing to a landmark @@ -212,7 +203,7 @@ namespace gtsam { */ Matrix AdjointMap() const; inline Vector Adjoint(const Vector& xi) const { - assert(xi.size() == 3); + assert(xi.size() == 3); return AdjointMap()*xi; } @@ -230,41 +221,41 @@ namespace gtsam { 0., 0., 0.); } - /** get functions for x, y, theta */ - inline double x() const { return t_.x(); } - inline double y() const { return t_.y(); } - inline double theta() const { return r_.theta(); } + /** get functions for x, y, theta */ + inline double x() const { return t_.x(); } + inline double y() const { return t_.y(); } + inline double theta() const { return r_.theta(); } - /** shorthand access functions */ - inline const Point2& t() const { return t_; } - inline const Rot2& r() const { return r_; } + /** shorthand access functions */ + inline const Point2& t() const { return t_; } + inline const Rot2& r() const { return r_; } - /** full access functions required by Pose concept */ - inline const Point2& translation() const { return t_; } - inline const Rot2& rotation() const { return r_; } + /** full access functions required by Pose concept */ + inline const Point2& translation() const { return t_; } + inline const Rot2& rotation() const { return r_; } - private: - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { +private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); - } - }; // Pose2 + } +}; // Pose2 - /** specialization for pose2 wedge function (generic template in Lie.h) */ - template <> - inline Matrix wedge(const Vector& xi) { - return Pose2::wedge(xi(0),xi(1),xi(2)); - } +/** specialization for pose2 wedge function (generic template in Lie.h) */ +template <> +inline Matrix wedge(const Vector& xi) { + return Pose2::wedge(xi(0),xi(1),xi(2)); +} - /** - * Calculate pose between a vector of 2D point correspondences (p,q) - * where q = Pose2::transform_from(p) = t + R*p - */ - typedef std::pair Point2Pair; - boost::optional align(const std::vector& pairs); +/** + * Calculate pose between a vector of 2D point correspondences (p,q) + * where q = Pose2::transform_from(p) = t + R*p + */ +typedef std::pair Point2Pair; +boost::optional align(const std::vector& pairs); } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index dc83247a7..99551db7a 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -95,17 +95,17 @@ namespace gtsam { } #ifdef CORRECT_POSE3_EXMAP - /* ************************************************************************* */ - // Changes default to use the full verions of expmap/logmap - /* ************************************************************************* */ - Pose3 Retract(const Vector& xi) { - return Pose3::Expmap(xi); - } - - /* ************************************************************************* */ - Vector Unretract(const Pose3& p) { - return Pose3::Logmap(p); - } +// /* ************************************************************************* */ +// // Changes default to use the full verions of expmap/logmap +// /* ************************************************************************* */ +// Pose3 Retract(const Vector& xi) { +// return Pose3::Expmap(xi); +// } +// +// /* ************************************************************************* */ +// Vector Unretract(const Pose3& p) { +// return Pose3::Logmap(p); +// } /* ************************************************************************* */ Pose3 retract(const Vector& d) { @@ -113,27 +113,27 @@ namespace gtsam { } /* ************************************************************************* */ - Vector unretract(const Pose3& T1, const Pose3& T2) { - return logmap(T2); + Vector localCoordinates(const Pose3& T1, const Pose3& T2) { + return localCoordinates(T2); } #else - /* ************************************************************************* */ - /* incorrect versions for which we know how to compute derivatives */ - Pose3 Pose3::Retract(const Vector& d) { - Vector w = sub(d, 0,3); - Vector u = sub(d, 3,6); - return Pose3(Rot3::Retract(w), Point3::Retract(u)); - } - - /* ************************************************************************* */ - // Log map at identity - return the translation and canonical rotation - // coordinates of a pose. - Vector Pose3::Unretract(const Pose3& p) { - const Vector w = Rot3::Unretract(p.rotation()), u = Point3::Unretract(p.translation()); - return concatVectors(2, &w, &u); - } +// /* ************************************************************************* */ +// /* incorrect versions for which we know how to compute derivatives */ +// Pose3 Pose3::Retract(const Vector& d) { +// Vector w = sub(d, 0,3); +// Vector u = sub(d, 3,6); +// return Pose3(Rot3::Retract(w), Point3::Retract(u)); +// } +// +// /* ************************************************************************* */ +// // Log map at identity - return the translation and canonical rotation +// // coordinates of a pose. +// Vector Pose3::Unretract(const Pose3& p) { +// const Vector w = Rot3::Unretract(p.rotation()), u = Point3::Unretract(p.translation()); +// return concatVectors(2, &w, &u); +// } /** These are the "old-style" expmap and logmap about the specified * pose. Increments the offset and rotation independently given a translation and @@ -145,9 +145,9 @@ namespace gtsam { } /** Independently computes the logmap of the translation and rotation. */ - Vector Pose3::unretract(const Pose3& pp) const { - const Vector r(R_.unretract(pp.rotation())), - t(t_.unretract(pp.translation())); + Vector Pose3::localCoordinates(const Pose3& pp) const { + const Vector r(R_.localCoordinates(pp.rotation())), + t(t_.localCoordinates(pp.translation())); return concatVectors(2, &r, &t); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 46c20061b..ba2e9a23e 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -91,6 +91,11 @@ namespace gtsam { /** Dimensionality of the tangent space */ inline size_t dim() const { return dimension; } + /** identity */ + inline static Pose3 identity() { + return Pose3(); + } + /** * Derivative of inverse */ @@ -115,28 +120,16 @@ namespace gtsam { Point3 transform_to(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** Exponential map at identity - create a pose with a translation and - * rotation (in canonical coordinates). */ - static Pose3 Retract(const Vector& v); - - /** Log map at identity - return the translation and canonical rotation - * coordinates of a pose. */ - static Vector Unretract(const Pose3& p); - /** Exponential map around another pose */ Pose3 retract(const Vector& d) const; /** Logarithm map around another pose T1 */ - Vector unretract(const Pose3& T2) const; + Vector localCoordinates(const Pose3& T2) const; /** non-approximated versions of Expmap/Logmap */ static Pose3 Expmap(const Vector& xi); static Vector Logmap(const Pose3& p); - /** non-approximated versions of expmap/logmap */ - inline Pose3 expmap(const Vector& v) const { return compose(Pose3::Expmap(v)); } - inline Vector logmap(const Pose3& p2) const { return Pose3::Logmap(between(p2));} - /** * Return relative pose between p1 and p2, in p1 coordinate frame * as well as optionally the derivatives diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index e4ac1582b..3a8fb86f0 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -127,6 +127,11 @@ namespace gtsam { return dimension; } + /** identity */ + inline static Rot2 identity() { + return Rot2(); + } + /** Compose - make a new rotation by adding angles */ inline Rot2 compose(const Rot2& R1, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { @@ -148,30 +153,14 @@ namespace gtsam { return Vector_(1, r.theta()); } - /** Binary expmap */ - inline Rot2 expmap(const Vector& v) const { - return *this * Expmap(v); - } - - /** Binary Logmap */ - inline Vector logmap(const Rot2& p2) const { - return Logmap(between(p2)); - } - // Manifold requirements - inline Rot2 retract(const Vector& v) const { return expmap(v); } - - /** expmap around identity */ - inline static Rot2 Retract(const Vector& v) { return Expmap(v); } + inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } /** * Returns inverse retraction */ - inline Vector unretract(const Rot2& t2) const { return logmap(t2); } - - /** Unretract around identity */ - inline static Vector Unretract(const Rot2& t) { return Logmap(t); } + inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } /** Between using the default implementation */ inline Rot2 between(const Rot2& p2, boost::optional H1 = diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 6ffb1f7d6..82a5ec39e 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -197,27 +197,22 @@ public: else return rodriguez(v); } + /** identity */ + inline static Rot3 identity() { + return Rot3(); + } + // Log map at identity - return the canonical coordinates of this rotation static Vector Logmap(const Rot3& R); - /** default implementations of binary functions */ - inline Rot3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); } - inline Vector logmap(const Rot3& p2) const { return gtsam::logmap_default(*this, p2);} - // Manifold requirements - inline Rot3 retract(const Vector& v) const { return expmap(v); } - - /** expmap around identity */ - inline static Rot3 Retract(const Vector& v) { return Expmap(v); } + inline Rot3 retract(const Vector& v) const { return compose(Expmap(v)); } /** * Returns inverse retraction */ - inline Vector unretract(const Rot3& t2) const { return logmap(t2); } - - /** Unretract around identity */ - inline static Vector Unretract(const Rot3& t) { return Logmap(t); } + inline Vector localCoordinates(const Rot3& t2) const { return Logmap(between(t2)); } // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3() diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index c00c5f392..cd3dbd19e 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -98,34 +98,19 @@ public: return 6; } - /** Exponential map around p0 */ - inline StereoCamera expmap(const Vector& d) const { - return StereoCamera(pose().retract(d), K_); - } - - Vector logmap(const StereoCamera &camera) const { - const Vector v1(leftCamPose_.unretract(camera.leftCamPose_)); - return v1; - } - - inline static StereoCamera Expmap(const Vector& d) { - return StereoCamera().expmap(d); - } - - inline static Vector Logmap(const StereoCamera &camera) { - return StereoCamera().logmap(camera); - } - bool equals(const StereoCamera &camera, double tol = 1e-9) const { return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( *camera.K_, tol); } // Manifold requirements - use existing expmap/logmap - inline StereoCamera retract(const Vector& v) const { return expmap(v); } - inline static StereoCamera Retract(const Vector& v) { return Expmap(v); } - inline Vector unretract(const StereoCamera& t2) const { return logmap(t2); } - inline static Vector Unretract(const StereoCamera& t) { return Logmap(t); } + inline StereoCamera retract(const Vector& v) const { + return StereoCamera(pose().retract(v), K_); + } + + inline Vector localCoordinates(const StereoCamera& t2) const { + return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_)); + } Pose3 between(const StereoCamera &camera, boost::optional H1=boost::none, diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index b2cb20cda..e493b0047 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -86,6 +86,11 @@ namespace gtsam { return *this + p1; } + /** identity */ + inline static StereoPoint2 identity() { + return StereoPoint2(); + } + /** inverse */ inline StereoPoint2 inverse() const { return StereoPoint2()- (*this); @@ -101,29 +106,14 @@ namespace gtsam { return p.vector(); } - /** default implementations of binary functions */ - inline StereoPoint2 expmap(const Vector& v) const { - return gtsam::expmap_default(*this, v); - } - - inline Vector logmap(const StereoPoint2& p2) const { - return gtsam::logmap_default(*this, p2); - } - // Manifold requirements - inline StereoPoint2 retract(const Vector& v) const { return expmap(v); } - - /** expmap around identity */ - inline static StereoPoint2 Retract(const Vector& v) { return Expmap(v); } + inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } /** * Returns inverse retraction */ - inline Vector unretract(const StereoPoint2& t2) const { return logmap(t2); } - - /** Unretract around identity */ - inline static Vector Unretract(const StereoPoint2& t) { return Logmap(t); } + inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } inline StereoPoint2 between(const StereoPoint2& p2) const { return gtsam::between_default(*this, p2); diff --git a/gtsam/geometry/tests/testHomography2.cpp b/gtsam/geometry/tests/testHomography2.cpp index 639462955..aa5aa4515 100644 --- a/gtsam/geometry/tests/testHomography2.cpp +++ b/gtsam/geometry/tests/testHomography2.cpp @@ -124,7 +124,7 @@ namespace gtsam { Index<3, 'C'> I; // contravariant 2D camera return toVector(H(I,_T)); } - Vector logmap(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) { + Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) { return toVector(A)-toVector(B); // TODO correct order ? } } diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index b4610d4fa..ec83c2ac7 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -23,7 +23,6 @@ using namespace std; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point2) -GTSAM_CONCEPT_MANIFOLD_INST(Point2) GTSAM_CONCEPT_LIE_INST(Point2) /* ************************************************************************* */ @@ -40,8 +39,8 @@ TEST(Point2, Lie) { EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(5,7), p1.expmap(Vector_(2, 4.,5.)))); - EXPECT(assert_equal(Vector_(2, 3.,3.), p1.logmap(p2))); + EXPECT(assert_equal(Point2(5,7), p1.retract(Vector_(2, 4.,5.)))); + EXPECT(assert_equal(Vector_(2, 3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ @@ -50,7 +49,7 @@ TEST( Point2, expmap) Vector d(2); d(0) = 1; d(1) = -1; - Point2 a(4, 5), b = a.expmap(d), c(5, 4); + Point2 a(4, 5), b = a.retract(d), c(5, 4); EXPECT(assert_equal(b,c)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 4e55c56c4..c7e25f7a9 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -21,7 +21,6 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point3) -GTSAM_CONCEPT_MANIFOLD_INST(Point3) GTSAM_CONCEPT_LIE_INST(Point3) Point3 P(0.2,0.7,-2); @@ -40,8 +39,8 @@ TEST(Point3, Lie) { EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5,7,9), p1.expmap(Vector_(3, 4.,5.,6.)))); - EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.logmap(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))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 1f3cd097f..0763cfba5 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -36,14 +36,8 @@ using namespace std; // #define SLOW_BUT_CORRECT_EXPMAP GTSAM_CONCEPT_TESTABLE_INST(Pose2) -GTSAM_CONCEPT_MANIFOLD_INST(Pose2) GTSAM_CONCEPT_LIE_INST(Pose2) -// concept checks for testable -GTSAM_CONCEPT_TESTABLE_INST(Point2) -GTSAM_CONCEPT_TESTABLE_INST(Rot2) -GTSAM_CONCEPT_TESTABLE_INST(LieVector) - /* ************************************************************************* */ TEST(Pose2, constructors) { Point2 p; @@ -59,10 +53,10 @@ TEST(Pose2, manifold) { Pose2 t1(M_PI_2, Point2(1, 2)); Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 origin; - Vector d12 = t1.unretract(t2); + Vector d12 = t1.localCoordinates(t2); EXPECT(assert_equal(t2, t1.retract(d12))); EXPECT(assert_equal(t2, t1*origin.retract(d12))); - Vector d21 = t2.unretract(t1); + Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); EXPECT(assert_equal(t1, t2*origin.retract(d21))); } @@ -83,7 +77,7 @@ TEST(Pose2, retract) { TEST(Pose2, expmap) { Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = pose.expmap(Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, Vector_(3, 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -91,7 +85,7 @@ TEST(Pose2, expmap) { TEST(Pose2, expmap2) { Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = pose.expmap(Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, Vector_(3, 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -117,12 +111,12 @@ TEST(Pose2, expmap3) { /* ************************************************************************* */ TEST(Pose2, expmap0) { Pose2 pose(M_PI_2, Point2(1, 2)); -#ifdef SLOW_BUT_CORRECT_EXPMAP +//#ifdef SLOW_BUT_CORRECT_EXPMAP Pose2 expected(1.01491, 2.01013, 1.5888); -#else - Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); -#endif - Pose2 actual = pose * Pose2::Retract(Vector_(3, 0.01, -0.015, 0.018)); +//#else +// Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); +//#endif + Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018))); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -183,7 +177,7 @@ TEST(Pose2, logmap) { #else Vector expected = Vector_(3, 0.01, -0.015, 0.018); #endif - Vector actual = pose0.unretract(pose); + Vector actual = pose0.localCoordinates(pose); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -192,7 +186,7 @@ TEST(Pose2, logmap_full) { Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01)); Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); - Vector actual = pose0.logmap(pose); + Vector actual = logmap_default(pose0, pose); EXPECT(assert_equal(expected, actual, 1e-5)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index c1ec39c3e..bbe69bc9e 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -25,7 +25,6 @@ using namespace std; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Pose3) -GTSAM_CONCEPT_MANIFOLD_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3) static Point3 P(0.2,0.7,-2); @@ -66,9 +65,9 @@ TEST( Pose3, expmap_a_full) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(id.expmap(v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; - EXPECT(assert_equal(Pose3(R, P),id.expmap(v),1e-5)); + EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } /* ************************************************************************* */ @@ -77,9 +76,9 @@ TEST( Pose3, expmap_a_full2) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(id.expmap(v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; - EXPECT(assert_equal(Pose3(R, P),id.expmap(v),1e-5)); + EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } /* ************************************************************************* */ @@ -215,11 +214,11 @@ Pose3 Agrawal06iros(const Vector& xi) { Vector v = xi.tail(3); double t = norm_2(w); if (t < 1e-5) - return Pose3(Rot3(), Point3::Retract(v)); + return Pose3(Rot3(), Point3::Expmap(v)); else { Matrix W = skewSymmetric(w/t); Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); - return Pose3(Rot3::Expmap (w), Point3::Retract(A * v)); + return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v)); } } @@ -526,12 +525,12 @@ TEST(Pose3, manifold) Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; - Vector d12 = t1.logmap(t2); - EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); // todo: richard - commented out because this tests for "compose-style" (new) expmap // EXPECT(assert_equal(t2, retract(origin,d12)*t1)); - Vector d21 = t2.logmap(t1); - EXPECT(assert_equal(t1, t2.expmap(d21))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); // todo: richard - commented out because this tests for "compose-style" (new) expmap // EXPECT(assert_equal(t1, retract(origin,d21)*t2)); @@ -656,9 +655,9 @@ TEST( Pose3, unicycle ) { // velocity in X should be X in inertial frame, rather than global frame Vector x_step = delta(6,3,1.0); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), x1.expmap(x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), x2.expmap(x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), x3.expmap(sqrt(2) * x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2) * x_step), tol)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index dc02de196..e23224898 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -23,7 +23,6 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Rot2) -GTSAM_CONCEPT_MANIFOLD_INST(Rot2) GTSAM_CONCEPT_LIE_INST(Rot2) Rot2 R(Rot2::fromAngle(0.1)); @@ -89,7 +88,7 @@ TEST( Rot2, equals) TEST( Rot2, expmap) { Vector v = zero(1); - CHECK(assert_equal(R.expmap(v), R)); + CHECK(assert_equal(R.retract(v), R)); } /* ************************************************************************* */ @@ -98,7 +97,7 @@ TEST(Rot2, logmap) Rot2 rot0(Rot2::fromAngle(M_PI_2)); Rot2 rot(Rot2::fromAngle(M_PI)); Vector expected = Vector_(1, M_PI_2); - Vector actual = rot0.logmap(rot); + Vector actual = rot0.localCoordinates(rot); CHECK(assert_equal(expected, actual)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 36a746dde..ec001b465 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -26,7 +26,6 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Rot3) -GTSAM_CONCEPT_MANIFOLD_INST(Rot3) GTSAM_CONCEPT_LIE_INST(Rot3) Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); @@ -142,7 +141,7 @@ TEST( Rot3, rodriguez4) TEST( Rot3, expmap) { Vector v = zero(3); - CHECK(assert_equal(R.expmap(v), R)); + CHECK(assert_equal(R.retract(v), R)); } /* ************************************************************************* */ @@ -189,11 +188,11 @@ TEST(Rot3, manifold) Rot3 origin; // log behaves correctly - Vector d12 = gR1.logmap(gR2); - CHECK(assert_equal(gR2, gR1.expmap(d12))); + Vector d12 = gR1.localCoordinates(gR2); + CHECK(assert_equal(gR2, gR1.retract(d12))); CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); - Vector d21 = gR2.logmap(gR1); - CHECK(assert_equal(gR1, gR2.expmap(d21))); + Vector d21 = gR2.localCoordinates(gR1); + CHECK(assert_equal(gR1, gR2.retract(d21))); CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); // Check that log(t1,t2)=-log(t2,t1) diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 26c484069..96410426d 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -15,7 +15,6 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) -GTSAM_CONCEPT_MANIFOLD_INST(StereoPoint2) GTSAM_CONCEPT_LIE_INST(StereoPoint2) /* ************************************************************************* */ diff --git a/gtsam/nonlinear/LieValues-inl.h b/gtsam/nonlinear/LieValues-inl.h index 18d3b207d..b5975e352 100644 --- a/gtsam/nonlinear/LieValues-inl.h +++ b/gtsam/nonlinear/LieValues-inl.h @@ -193,19 +193,19 @@ namespace gtsam { // todo: insert for every element is inefficient // todo: currently only logmaps elements in both configs template - inline VectorValues LieValues::unretract(const LieValues& cp, const Ordering& ordering) const { + inline VectorValues LieValues::localCoordinates(const LieValues& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); - unretract(cp, ordering, delta); + localCoordinates(cp, ordering, delta); return delta; } /* ************************************************************************* */ template - void LieValues::unretract(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const { + void LieValues::localCoordinates(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const { typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, cp) { assert(this->exists(value.first)); - delta[ordering[value.first]] = this->at(value.first).unretract(value.second); + delta[ordering[value.first]] = this->at(value.first).localCoordinates(value.second); } } diff --git a/gtsam/nonlinear/LieValues.h b/gtsam/nonlinear/LieValues.h index 4e6c13cb0..b6b3ffdf4 100644 --- a/gtsam/nonlinear/LieValues.h +++ b/gtsam/nonlinear/LieValues.h @@ -122,10 +122,10 @@ namespace gtsam { LieValues retract(const VectorValues& delta, const Ordering& ordering) const; /** Get a delta config about a linearization point c0 (*this) */ - VectorValues unretract(const LieValues& cp, const Ordering& ordering) const; + VectorValues localCoordinates(const LieValues& cp, const Ordering& ordering) const; /** Get a delta config about a linearization point c0 (*this) */ - void unretract(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const; + void localCoordinates(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const; // imperative methods: diff --git a/gtsam/nonlinear/NonlinearConstraint.h b/gtsam/nonlinear/NonlinearConstraint.h index d41c40147..9478d808d 100644 --- a/gtsam/nonlinear/NonlinearConstraint.h +++ b/gtsam/nonlinear/NonlinearConstraint.h @@ -573,7 +573,7 @@ public: Vector evaluateError(const X& x1, boost::optional H1 = boost::none) const { const size_t p = X::Dim(); if (H1) *H1 = eye(p); - return value_.unretract(x1); + return value_.localCoordinates(x1); } /** Print */ @@ -628,7 +628,7 @@ public: const size_t p = X::Dim(); if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); - return x1.unretract(x2); + return x1.localCoordinates(x2); } private: diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index f68674dc0..2b7ac2e7f 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -120,7 +120,7 @@ namespace gtsam { size_t nj = feasible_.dim(); if (allow_error_) { if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare - return xj.unretract(feasible_); + return xj.localCoordinates(feasible_); } else if (compare_(feasible_,xj)) { if (H) *H = eye(nj); return zero(nj); // set error to zero if equal diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h index e282cc226..c4d701c35 100644 --- a/gtsam/nonlinear/TupleValues.h +++ b/gtsam/nonlinear/TupleValues.h @@ -210,16 +210,16 @@ namespace gtsam { } /** logmap each element */ - VectorValues unretract(const TupleValues& cp, const Ordering& ordering) const { + VectorValues localCoordinates(const TupleValues& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); - unretract(cp, ordering, delta); + localCoordinates(cp, ordering, delta); return delta; } /** logmap each element */ - void unretract(const TupleValues& cp, const Ordering& ordering, VectorValues& delta) const { - first_.unretract(cp.first_, ordering, delta); - second_.unretract(cp.second_, ordering, delta); + void localCoordinates(const TupleValues& cp, const Ordering& ordering, VectorValues& delta) const { + first_.localCoordinates(cp.first_, ordering, delta); + second_.localCoordinates(cp.second_, ordering, delta); } /** @@ -322,14 +322,14 @@ namespace gtsam { return TupleValuesEnd(first_.retract(delta, ordering)); } - VectorValues unretract(const TupleValuesEnd& cp, const Ordering& ordering) const { + VectorValues localCoordinates(const TupleValuesEnd& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); - unretract(cp, ordering, delta); + localCoordinates(cp, ordering, delta); return delta; } - void unretract(const TupleValuesEnd& cp, const Ordering& ordering, VectorValues& delta) const { - first_.unretract(cp.first_, ordering, delta); + void localCoordinates(const TupleValuesEnd& cp, const Ordering& ordering, VectorValues& delta) const { + first_.localCoordinates(cp.first_, ordering, delta); } /** diff --git a/gtsam/slam/BetweenConstraint.h b/gtsam/slam/BetweenConstraint.h index 33b1ab524..e4085f0dc 100644 --- a/gtsam/slam/BetweenConstraint.h +++ b/gtsam/slam/BetweenConstraint.h @@ -51,7 +51,7 @@ namespace gtsam { boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { X hx = x1.between(x2, H1, H2); - return measured_.unretract(hx); + return measured_.localCoordinates(hx); } inline const X measured() const { diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index c8fddbea0..c12b65d50 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -83,7 +83,7 @@ namespace gtsam { boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) - return measured_.unretract(hx); + return measured_.localCoordinates(hx); } /** return the measured */ diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 7e728370c..66ec820c4 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -82,7 +82,7 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const { - Vector error = z_.unretract(camera.project(point,H1,H2)); + Vector error = z_.localCoordinates(camera.project(point,H1,H2)); // gtsam::print(error, "error"); return error; } diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 08edc7dc8..aa59d436d 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -118,16 +118,17 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { if (H) (*H) = zeros(this->dim(), p.dim()); - Vector full_unretraction = T::Unretract(p); - Vector masked_unretraction = zero(this->dim()); + // FIXME: this was originally the generic retraction - may not produce same results + Vector full_logmap = T::Logmap(p); + Vector masked_logmap = zero(this->dim()); size_t masked_idx=0; for (size_t i=0;i H = boost::none) const { if (H) (*H) = eye(p.dim()); // manifold equivalent of h(x)-z -> log(z,h(x)) - return prior_.unretract(p); + return prior_.localCoordinates(p); } private: diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 08dc30e43..b85b479c3 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -69,7 +69,7 @@ namespace gtsam { /// Evaluate error and optionally derivative Vector evaluateError(const Pose2& x, boost::optional H = boost::none) const { - return z_.unretract(prior(x, H)); + return z_.localCoordinates(prior(x, H)); } }; @@ -93,7 +93,7 @@ namespace gtsam { Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return z_.unretract(odo(x1, x2, H1, H2)); + return z_.localCoordinates(odo(x1, x2, H1, H2)); } }; diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 7049baaf9..ad9550304 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -140,9 +140,9 @@ TEST( Pose3Factor, error ) x.insert(1,t1); x.insert(2,t2); - // Get error h(x)-z -> unretract(z,h(x)) = unretract(z,between(t1,t2)) + // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) Vector actual = factor.unwhitenedError(x); - Vector expected = z.unretract(t1.between(t2)); + Vector expected = z.localCoordinates(t1.between(t2)); CHECK(assert_equal(expected,actual)); } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 2ea5cf12f..e05ccde99 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -232,7 +232,7 @@ public: } // Return the error between the prediction and the supplied value of p2 - return prediction.unretract(p2); + return prediction.localCoordinates(p2); } }; diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp index f95013421..4655ab303 100644 --- a/tests/testTupleValues.cpp +++ b/tests/testTupleValues.cpp @@ -203,7 +203,7 @@ TEST(TupleValues, zero_expmap_logmap) // Check log VectorValues expected_log = delta; - VectorValues actual_log = values1.unretract(actual, o); + VectorValues actual_log = values1.localCoordinates(actual, o); CHECK(assert_equal(expected_log, actual_log)); } @@ -460,7 +460,7 @@ TEST(TupleValues, expmap) expected.insert(l2k, Point2(10.3, 11.4)); CHECK(assert_equal(expected, values1.retract(delta, o))); - CHECK(assert_equal(delta, values1.unretract(expected, o))); + CHECK(assert_equal(delta, values1.localCoordinates(expected, o))); } /* ************************************************************************* */ @@ -491,7 +491,7 @@ TEST(TupleValues, expmap_typedefs) expected.insert(l2k, Point2(10.3, 11.4)); CHECK(assert_equal(expected, TupleValues2(values1.retract(delta, o)))); - //CHECK(assert_equal(delta, values1.unretract(expected))); + //CHECK(assert_equal(delta, values1.localCoordinates(expected))); } /* ************************************************************************* */