Updated a few lie group function comments

release/4.3a0
Nick Barrash 2012-01-11 03:04:35 +00:00
parent d7317f4eb6
commit 8c1695a247
5 changed files with 37 additions and 16 deletions

View File

@ -139,10 +139,10 @@ public:
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
/// Exponential map from Lie algebra se(2) to SE(2) ///Exponential map at identity - create a rotation from canonical coordinates
static Pose2 Expmap(const Vector& xi); static Pose2 Expmap(const Vector& xi);
/// Exponential map from SE(2) to Lie algebra se(2) ///Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Pose2& p); static Vector Logmap(const Pose2& p);
/// @} /// @}

View File

@ -32,6 +32,7 @@ namespace gtsam {
/** /**
* A 3D pose (R,t) : (Rot3,Point3) * A 3D pose (R,t) : (Rot3,Point3)
* @ingroup geometry * @ingroup geometry
* \nosubgrouping
*/ */
class Pose3 { class Pose3 {
public: public:
@ -47,6 +48,9 @@ namespace gtsam {
public: public:
/// @name Standard Constructors
/// @{
/** Default constructor is origin */ /** Default constructor is origin */
Pose3() {} Pose3() {}
@ -64,16 +68,7 @@ namespace gtsam {
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {}
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;
/// @name Testable /// @name Testable
/// @{ /// @{
@ -134,13 +129,25 @@ namespace gtsam {
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
/// Exponential map from Lie algebra se(3) to SE(3) /// Exponential map at identity - create a rotation from canonical coordinates
static Pose3 Expmap(const Vector& xi); static Pose3 Expmap(const Vector& xi);
/// Exponential map from SE(3) to Lie algebra se(3) /// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Pose3& p); 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 */ /** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const { return transform_from(p); } inline Point3 operator*(const Point3& p) const { return transform_from(p); }
@ -203,6 +210,10 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const; boost::optional<Matrix&> H2=boost::none) const;
/// @}
/// @name Advanced Interface
/// @{
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -224,5 +235,6 @@ namespace gtsam {
inline Matrix wedge<Pose3>(const Vector& xi) { inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
} }
/// @}
} // namespace gtsam } // namespace gtsam

View File

@ -155,7 +155,7 @@ namespace gtsam {
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
/// Expmap around identity - create a rotation from an angle ///Exponential map at identity - create a rotation from canonical coordinates
static Rot2 Expmap(const Vector& v) { static Rot2 Expmap(const Vector& v) {
if (zero(v)) if (zero(v))
return (Rot2()); return (Rot2());
@ -163,7 +163,7 @@ namespace gtsam {
return Rot2::fromAngle(v(0)); return Rot2::fromAngle(v(0));
} }
/// Logmap around identity - return the angle of the rotation ///Log map at identity - return the canonical coordinates of this rotation
static inline Vector Logmap(const Rot2& r) { static inline Vector Logmap(const Rot2& r) {
return Vector_(1, r.theta()); return Vector_(1, r.theta());
} }

View File

@ -281,6 +281,8 @@ namespace gtsam {
static Vector Logmap(const Rot3& R); static Vector Logmap(const Rot3& R);
/// @} /// @}
/// @name Standard Interface
/// @{
/** return 3*3 rotation matrix */ /** return 3*3 rotation matrix */
Matrix matrix() const; Matrix matrix() const;
@ -336,6 +338,10 @@ namespace gtsam {
*/ */
inline double yaw() const { return ypr()(0); } inline double yaw() const { return ypr()(0); }
/// @}
/// @name Advanced Interface
/// @{
/** Compute the quaternion representation of this rotation. /** Compute the quaternion representation of this rotation.
* @return The quaternion * @return The quaternion
*/ */
@ -357,6 +363,8 @@ namespace gtsam {
} }
}; };
/// @}
/** /**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'

View File

@ -132,6 +132,7 @@ namespace gtsam {
return Point2(uL_, v_); return Point2(uL_, v_);
} }
///TODO comment
inline StereoPoint2 between(const StereoPoint2& p2) const { inline StereoPoint2 between(const StereoPoint2& p2) const {
return gtsam::between_default(*this, p2); return gtsam::between_default(*this, p2);
} }