12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

geometry doc fixes 2.0

release/4.3a0
Nick Barrash 2012-01-24 04:13:16 +00:00
parent 14991601c3
commit 5010f016c7
5 changed files with 77 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
gtsam/geometry/Point3.h
View File

@ -84,6 +84,9 @@ namespace gtsam {
/// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3()
inline Point3 inverse() const { return Point3(-x_, -y_, -z_); }
/// syntactic sugar for inverse, i.e., -p == inverse(p)
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
/// "Compose" - just adds coordinates of two points
inline Point3 compose(const Point3& p2,
boost::optional<Matrix&> H1=boost::none,
@ -93,6 +96,21 @@ namespace gtsam {
return *this + p2;
}
///syntactic sugar for adding two points, i.e., p+q == compose(p,q)
Point3 operator + (const Point3& q) const;
/** Between using the default implementation */
inline Point3 between(const Point3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(3);
if(H2) *H2 = eye(3);
return p2 - *this;
}
/// syntactic sugar for subtracting points, i.e., q-p == between(p,q)
Point3 operator - (const Point3& q) const;
/// @}
/// @name Manifold
/// @{
@ -120,25 +138,13 @@ namespace gtsam {
static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
/// @}
/// @name Vector Operators
/// @name Vector Space
/// @{
///TODO: comment
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
///TODO: comment
bool operator ==(const Point3& q) const;
///TODO: comment
Point3 operator + (const Point3& q) const;
///TODO: comment
Point3 operator - (const Point3& q) const;
///TODO: comment
///multiply with a scalar
Point3 operator * (double s) const;
///TODO: comment
///divide by a scalar
Point3 operator / (double s) const;
/** distance between two points */
@ -159,14 +165,8 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/** Between using the default implementation */
inline Point3 between(const Point3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(3);
if(H2) *H2 = eye(3);
return p2 - *this;
}
/// equality
bool operator ==(const Point3& q) const;
/** return vectorized form (column-wise)*/
Vector vector() const {
@ -191,9 +191,10 @@ namespace gtsam {
Point3 sub (const Point3 &q,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/// @}
private:
/// @}
/// @name Advanced Interface
/// @{
@ -206,11 +207,12 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(y_);
ar & BOOST_SERIALIZATION_NVP(z_);
}
/// @}
};
/// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;}
/// @}
}

18
gtsam/geometry/Pose2.h
View File

@ -71,7 +71,7 @@ public:
r_(Rot2::fromAngle(theta)), t_(t) {
}
///TODO: comment
/** construct from r,t */
Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
/** Constructor from 3*3 matrix */
@ -119,6 +119,14 @@ public:
return Pose2(r_*p2.r(), t_ + r_*p2.t());
}
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// @}
/// @name Manifold
/// @{
@ -176,13 +184,6 @@ public:
/** return transformation matrix */
Matrix matrix() const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Return point coordinates in pose coordinate frame
*/
@ -252,6 +253,7 @@ public:
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_; }

30
gtsam/geometry/Pose3.h
View File

@ -98,6 +98,14 @@ namespace gtsam {
return Pose3(R_*T.R_, t_ + R_*T.t_);
}
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// @}
/// @name Manifold
/// @{
@ -135,14 +143,6 @@ namespace gtsam {
/// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Pose3& p);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate Adjoint map
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
@ -169,25 +169,33 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/// get rotation
const Rot3& rotation() const { return R_; }
/// get translation
const Point3& translation() const { return t_; }
/// get x
double x() const { return t_.x(); }
/// get y
double y() const { return t_.y(); }
/// get z
double z() const { return t_.z(); }
/** convert to 4*4 matrix */
Matrix matrix() const;
/** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
Pose3 transform_to(const Pose3& pose) const;
/** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;

24
gtsam/geometry/Rot2.h
View File

@ -126,9 +126,12 @@ namespace gtsam {
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
/** syntactic sugar for rotate */
inline Point2 operator*(const Point2& p) const {
return rotate(p);
/** Between using the default implementation */
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = -eye(1);
if (H2) *H2 = eye(1);
return between_default(*this, p2);
}
/// @}
@ -169,7 +172,7 @@ namespace gtsam {
}
/// @}
/// @name Vector Operators
/// @name Vector Space
/// @{
/**
@ -204,14 +207,6 @@ namespace gtsam {
return s_;
}
/** Between using the default implementation */
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = -eye(1);
if (H2) *H2 = eye(1);
return between_default(*this, p2);
}
/** return 2*2 rotation matrix */
Matrix matrix() const;
@ -225,6 +220,11 @@ namespace gtsam {
Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
/** syntactic sugar for rotate */
inline Point2 operator*(const Point2& p) const {
return rotate(p);
}
/**
* rotate point from world to rotated
* frame = R'*p

16
gtsam/geometry/Rot3.h
View File

@ -193,15 +193,15 @@ namespace gtsam {
return Rot3();
}
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const;
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const;
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
@ -210,9 +210,6 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const;
/// @}
/// @name Manifold
/// @{
@ -280,6 +277,9 @@ namespace gtsam {
Point3 rotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const;
/**
* rotate point from world to rotated
* frame = R'*p