Updated a few lie group function comments
parent
d7317f4eb6
commit
8c1695a247
|
|
@ -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);
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
|
||||||
|
|
@ -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());
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -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'
|
||||||
|
|
|
||||||
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue