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

fixed some comment groupings in geometry

release/4.3a0
Nick Barrash 2012-01-24 02:27:44 +00:00
parent aa940ec8d0
commit 4e2d7a69bd
3 changed files with 66 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/geometry/Pose2.h
View File

@ -145,6 +145,30 @@ public:
///Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Pose2& p);
/**
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
*/
Matrix AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {
assert(xi.size() == 3);
return AdjointMap()*xi;
}
/**
* wedge for SE(2):
* @param xi 3-dim twist (v,omega) where
* omega is angular velocity
* v (vx,vy) = 2D velocity
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/
static inline Matrix wedge(double vx, double vy, double w) {
return Matrix_(3,3,
0.,-w, vx,
w, 0., vy,
0., 0., 0.);
}
/// @}
/// @name Standard Interface
/// @{
@ -212,30 +236,6 @@ public:
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
*/
Matrix AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {
assert(xi.size() == 3);
return AdjointMap()*xi;
}
/**
* wedge for SE(2):
* @param xi 3-dim twist (v,omega) where
* omega is angular velocity
* v (vx,vy) = 2D velocity
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/
static inline Matrix wedge(double vx, double vy, double w) {
return Matrix_(3,3,
0.,-w, vx,
w, 0., vy,
0., 0., 0.);
}
/** get functions for x, y, theta */
/// get x

54
gtsam/geometry/Pose3.h
View File

@ -135,33 +135,6 @@ namespace gtsam {
/// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Pose3& p);
/// @}
/// @name Standard Interface
/// @{
const Rot3& rotation() const { return R_; }
const Point3& translation() const { return t_; }
double x() const { return t_.x(); }
double y() const { return t_.y(); }
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;
/** 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;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
@ -192,6 +165,33 @@ namespace gtsam {
0., 0., 0., 0.);
}
/// @}
/// @name Standard Interface
/// @{
const Rot3& rotation() const { return R_; }
const Point3& translation() const { return t_; }
double x() const { return t_.x(); }
double y() const { return t_.y(); }
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;
/** 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;
/**
* Calculate range to a landmark
* @param point 3D location of landmark

29
gtsam/geometry/Rot3.h
View File

@ -213,20 +213,6 @@ namespace gtsam {
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const;
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/// @}
/// @name Manifold
/// @{
@ -287,6 +273,21 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** return 3*3 rotation matrix */
Matrix matrix() const;