|
|
|
@ -23,142 +23,142 @@
|
|
|
|
|
|
|
|
|
|
namespace gtsam {
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* A 3D pose (R,t) : (Rot3,Point3)
|
|
|
|
|
* @ingroup geometry
|
|
|
|
|
*/
|
|
|
|
|
class Pose3 {
|
|
|
|
|
public:
|
|
|
|
|
static const size_t dimension = 6;
|
|
|
|
|
/**
|
|
|
|
|
* A 3D pose (R,t) : (Rot3,Point3)
|
|
|
|
|
* @ingroup geometry
|
|
|
|
|
*/
|
|
|
|
|
class Pose3 {
|
|
|
|
|
public:
|
|
|
|
|
static const size_t dimension = 6;
|
|
|
|
|
|
|
|
|
|
/** Pose Concept requirements */
|
|
|
|
|
typedef Rot3 Rotation;
|
|
|
|
|
typedef Point3 Translation;
|
|
|
|
|
/** Pose Concept requirements */
|
|
|
|
|
typedef Rot3 Rotation;
|
|
|
|
|
typedef Point3 Translation;
|
|
|
|
|
|
|
|
|
|
private:
|
|
|
|
|
Rot3 R_;
|
|
|
|
|
Point3 t_;
|
|
|
|
|
private:
|
|
|
|
|
Rot3 R_;
|
|
|
|
|
Point3 t_;
|
|
|
|
|
|
|
|
|
|
public:
|
|
|
|
|
public:
|
|
|
|
|
|
|
|
|
|
/** Default constructor is origin */
|
|
|
|
|
Pose3() {}
|
|
|
|
|
/** Default constructor is origin */
|
|
|
|
|
Pose3() {}
|
|
|
|
|
|
|
|
|
|
/** Copy constructor */
|
|
|
|
|
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {}
|
|
|
|
|
/** Copy constructor */
|
|
|
|
|
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {}
|
|
|
|
|
|
|
|
|
|
/** Construct from R,t */
|
|
|
|
|
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
|
|
|
|
|
/** Construct from R,t */
|
|
|
|
|
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
|
|
|
|
|
|
|
|
|
|
/** Constructor from 4*4 matrix */
|
|
|
|
|
Pose3(const Matrix &T) :
|
|
|
|
|
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)) {}
|
|
|
|
|
/** Constructor from 4*4 matrix */
|
|
|
|
|
Pose3(const Matrix &T) :
|
|
|
|
|
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)) {}
|
|
|
|
|
|
|
|
|
|
/** Constructor from 12D vector */
|
|
|
|
|
Pose3(const Vector &V) :
|
|
|
|
|
R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)),
|
|
|
|
|
t_(V(9), V(10),V(11)) {}
|
|
|
|
|
/** Constructor from 12D vector */
|
|
|
|
|
Pose3(const Vector &V) :
|
|
|
|
|
R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)),
|
|
|
|
|
t_(V(9), V(10),V(11)) {}
|
|
|
|
|
|
|
|
|
|
inline const Rot3& rotation() const { return R_; }
|
|
|
|
|
inline const Point3& translation() const { return t_; }
|
|
|
|
|
inline const Rot3& rotation() const { return R_; }
|
|
|
|
|
inline const Point3& translation() const { return t_; }
|
|
|
|
|
|
|
|
|
|
inline double x() const { return t_.x(); }
|
|
|
|
|
inline double y() const { return t_.y(); }
|
|
|
|
|
inline double z() const { return t_.z(); }
|
|
|
|
|
inline double x() const { return t_.x(); }
|
|
|
|
|
inline double y() const { return t_.y(); }
|
|
|
|
|
inline double z() const { return t_.z(); }
|
|
|
|
|
|
|
|
|
|
/** convert to 4*4 matrix */
|
|
|
|
|
Matrix matrix() const;
|
|
|
|
|
/** convert to 4*4 matrix */
|
|
|
|
|
Matrix matrix() const;
|
|
|
|
|
|
|
|
|
|
/** 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 Pose3& pose, double tol = 1e-9) const;
|
|
|
|
|
/** assert equality up to a tolerance */
|
|
|
|
|
bool equals(const Pose3& pose, double tol = 1e-9) const;
|
|
|
|
|
|
|
|
|
|
/** Compose two poses */
|
|
|
|
|
inline Pose3 operator*(const Pose3& T) const {
|
|
|
|
|
return Pose3(R_*T.R_, t_ + R_*T.t_);
|
|
|
|
|
}
|
|
|
|
|
/** Compose two poses */
|
|
|
|
|
inline Pose3 operator*(const Pose3& T) const {
|
|
|
|
|
return Pose3(R_*T.R_, t_ + R_*T.t_);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
Pose3 transform_to(const Pose3& pose) const;
|
|
|
|
|
Pose3 transform_to(const Pose3& pose) const;
|
|
|
|
|
|
|
|
|
|
/** 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 */
|
|
|
|
|
|
|
|
|
|
/** Dimensionality of the tangent space */
|
|
|
|
|
inline size_t dim() const { return dimension; }
|
|
|
|
|
/** Dimensionality of the tangent space */
|
|
|
|
|
inline size_t dim() const { return dimension; }
|
|
|
|
|
|
|
|
|
|
/** identity */
|
|
|
|
|
inline static Pose3 identity() {
|
|
|
|
|
return Pose3();
|
|
|
|
|
}
|
|
|
|
|
/** identity */
|
|
|
|
|
inline static Pose3 identity() {
|
|
|
|
|
return Pose3();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Derivative of inverse
|
|
|
|
|
*/
|
|
|
|
|
Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const;
|
|
|
|
|
/**
|
|
|
|
|
* Derivative of inverse
|
|
|
|
|
*/
|
|
|
|
|
Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const;
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* composes two poses (first (*this) then p2)
|
|
|
|
|
* with optional derivatives
|
|
|
|
|
*/
|
|
|
|
|
Pose3 compose(const Pose3& p2,
|
|
|
|
|
boost::optional<Matrix&> H1=boost::none,
|
|
|
|
|
boost::optional<Matrix&> H2=boost::none) const;
|
|
|
|
|
/**
|
|
|
|
|
* composes two poses (first (*this) then p2)
|
|
|
|
|
* with optional derivatives
|
|
|
|
|
*/
|
|
|
|
|
Pose3 compose(const Pose3& p2,
|
|
|
|
|
boost::optional<Matrix&> H1=boost::none,
|
|
|
|
|
boost::optional<Matrix&> H2=boost::none) 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 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 */
|
|
|
|
|
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
|
|
|
|
|
/** syntactic sugar for transform */
|
|
|
|
|
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;
|
|
|
|
|
/** 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;
|
|
|
|
|
|
|
|
|
|
/** Exponential map around another pose */
|
|
|
|
|
Pose3 retract(const Vector& d) const;
|
|
|
|
|
/** Exponential map around another pose */
|
|
|
|
|
Pose3 retract(const Vector& d) const;
|
|
|
|
|
|
|
|
|
|
/** Logarithm map around another pose T1 */
|
|
|
|
|
Vector localCoordinates(const Pose3& T2) const;
|
|
|
|
|
/** Logarithm map around another pose T1 */
|
|
|
|
|
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 */
|
|
|
|
|
static Pose3 Expmap(const Vector& xi);
|
|
|
|
|
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;
|
|
|
|
|
/**
|
|
|
|
|
* 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)
|
|
|
|
|
*/
|
|
|
|
|
Matrix AdjointMap() const;
|
|
|
|
|
inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
|
|
|
|
|
/**
|
|
|
|
|
* Calculate Adjoint map
|
|
|
|
|
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
|
|
|
|
|
*/
|
|
|
|
|
Matrix AdjointMap() const;
|
|
|
|
|
inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* wedge for Pose3:
|
|
|
|
|
* @param xi 6-dim twist (omega,v) where
|
|
|
|
|
* omega = (wx,wy,wz) 3D angular velocity
|
|
|
|
|
* v (vx,vy,vz) = 3D velocity
|
|
|
|
|
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
|
|
|
|
|
*/
|
|
|
|
|
static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
|
|
|
|
|
return Matrix_(4,4,
|
|
|
|
|
0.,-wz, wy, vx,
|
|
|
|
|
wz, 0.,-wx, vy,
|
|
|
|
|
-wy, wx, 0., vz,
|
|
|
|
|
0., 0., 0., 0.);
|
|
|
|
|
}
|
|
|
|
|
/**
|
|
|
|
|
* wedge for Pose3:
|
|
|
|
|
* @param xi 6-dim twist (omega,v) where
|
|
|
|
|
* omega = (wx,wy,wz) 3D angular velocity
|
|
|
|
|
* v (vx,vy,vz) = 3D velocity
|
|
|
|
|
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
|
|
|
|
|
*/
|
|
|
|
|
static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
|
|
|
|
|
return Matrix_(4,4,
|
|
|
|
|
0.,-wz, wy, vx,
|
|
|
|
|
wz, 0.,-wx, vy,
|
|
|
|
|
-wy, wx, 0., vz,
|
|
|
|
|
0., 0., 0., 0.);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Calculate range to a landmark
|
|
|
|
@ -178,26 +178,26 @@ namespace gtsam {
|
|
|
|
|
boost::optional<Matrix&> H1=boost::none,
|
|
|
|
|
boost::optional<Matrix&> H2=boost::none) const;
|
|
|
|
|
|
|
|
|
|
private:
|
|
|
|
|
/** Serialization function */
|
|
|
|
|
friend class boost::serialization::access;
|
|
|
|
|
template<class Archive>
|
|
|
|
|
void serialize(Archive & ar, const unsigned int version) {
|
|
|
|
|
ar & BOOST_SERIALIZATION_NVP(R_);
|
|
|
|
|
ar & BOOST_SERIALIZATION_NVP(t_);
|
|
|
|
|
}
|
|
|
|
|
}; // Pose3 class
|
|
|
|
|
private:
|
|
|
|
|
/** Serialization function */
|
|
|
|
|
friend class boost::serialization::access;
|
|
|
|
|
template<class Archive>
|
|
|
|
|
void serialize(Archive & ar, const unsigned int version) {
|
|
|
|
|
ar & BOOST_SERIALIZATION_NVP(R_);
|
|
|
|
|
ar & BOOST_SERIALIZATION_NVP(t_);
|
|
|
|
|
}
|
|
|
|
|
}; // Pose3 class
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* wedge for Pose3:
|
|
|
|
|
* @param xi 6-dim twist (omega,v) where
|
|
|
|
|
* omega = 3D angular velocity
|
|
|
|
|
* v = 3D velocity
|
|
|
|
|
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
|
|
|
|
|
*/
|
|
|
|
|
template <>
|
|
|
|
|
inline Matrix wedge<Pose3>(const Vector& xi) {
|
|
|
|
|
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
|
|
|
|
|
}
|
|
|
|
|
/**
|
|
|
|
|
* wedge for Pose3:
|
|
|
|
|
* @param xi 6-dim twist (omega,v) where
|
|
|
|
|
* omega = 3D angular velocity
|
|
|
|
|
* v = 3D velocity
|
|
|
|
|
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
|
|
|
|
|
*/
|
|
|
|
|
template <>
|
|
|
|
|
inline Matrix wedge<Pose3>(const Vector& xi) {
|
|
|
|
|
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
} // namespace gtsam
|
|
|
|
|