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

Removed wrap function duplicates due to shared_ptr, removed the wrapping functions inside geometry classes, added named constructors to the wrappable functions

release/4.3a0
Alex Cunningham 2011-12-07 15:23:20 +00:00
parent 92a0cf67c9
commit 886f9459b4
7 changed files with 22 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam.h
View File

@ -26,8 +26,10 @@
/**
* Status:
* - Depreciated versions of functions that return a shared_ptr unnecessarily are still present
* - TODO: global functions
* - TODO: default values for arguments
* - TODO: overloaded functions
* - TODO: Handle Rot3M conversions to quaternions
* - TODO: namespace detection to handle nested namespaces
*/
@ -43,12 +45,6 @@ class Point2 {
Point2 compose(const Point2& p2);
Point2 between(const Point2& p2);
Point2 retract(Vector v);
// Depreciated interface
Point2* compose_(const Point2& p2);
Point2* between_(const Point2& p2);
Point2* retract_(Vector v);
static Point2* Expmap_(Vector v);
};
class Point3 {
@ -67,12 +63,6 @@ class Point3 {
Point3 retract(Vector v);
Point3 compose(const Point3& p2);
Point3 between(const Point3& p2);
// Depreciated interface
static Point3* Expmap_(Vector v);
Point3* compose_(const Point3& p2);
Point3* between_(const Point3& p2);
Point3* retract_(Vector v);
};
class Rot2 {
@ -80,20 +70,21 @@ class Rot2 {
Rot2(double theta);
static Rot2 Expmap(Vector v);
static Vector Logmap(const Rot2& p);
static Rot2 fromAngle(double theta);
static Rot2 fromDegrees(double theta);
static Rot2 fromCosSin(double c, double s);
static Rot2 relativeBearing(const Point2& d); // Ignoring derivative
static Rot2 atan2(double y, double x);
void print(string s) const;
bool equals(const Rot2& rot, double tol) const;
double theta() const;
double degrees() const;
double c() const;
double s() const;
Vector localCoordinates(const Rot2& p);
Rot2 retract(Vector v);
Rot2 compose(const Rot2& p2);
Rot2 between(const Rot2& p2);
// Depreciated interface
Rot2* compose_(const Rot2& p2);
Rot2* between_(const Rot2& p2);
Rot2* retract_(Vector v);
static Rot2* Expmap_(Vector v);
};
class Rot3 {
@ -102,22 +93,27 @@ class Rot3 {
static Rot3 Expmap(Vector v);
static Vector Logmap(const Rot3& p);
static Rot3 ypr(double y, double p, double r);
static Rot3 Rx(double t);
static Rot3 Ry(double t);
static Rot3 Rz(double t);
static Rot3 RzRyRx(double x, double y, double z);
static Rot3 RzRyRx(const Vector& xyz);
static Rot3 yaw (double t); // positive yaw is to right (as in aircraft heading)
static Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude)
static Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft)
static Rot3 quaternion(double w, double x, double y, double z);
static Rot3 rodriguez(const Vector& v);
Matrix matrix() const;
Matrix transpose() const;
Vector xyz() const;
Vector ypr() const;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
void print(string s) const;
bool equals(const Rot3& rot, double tol) const;
Vector localCoordinates(const Rot3& p);
Rot3 retract(Vector v);
Rot3 compose(const Rot3& p2);
Rot3 between(const Rot3& p2);
// Depreciated interface
Rot3* compose_(const Rot3& p2);
Rot3* between_(const Rot3& p2);
Rot3* retract_(Vector v);
static Rot3* Expmap_(Vector v);
};
class Pose2 {
@ -138,12 +134,6 @@ class Pose2 {
Pose2 retract(Vector v);
Pose2 compose(const Pose2& p2);
Pose2 between(const Pose2& p2);
// Depreciated interface
Pose2* compose_(const Pose2& p2);
Pose2* between_(const Pose2& p2);
Pose2* retract_(Vector v);
static Pose2* Expmap_(Vector v);
};
class Pose3 {
@ -165,14 +155,6 @@ class Pose3 {
Pose3 retract(Vector v);
Point3 translation() const;
Rot3 rotation() const;
// Depreciated interface
static Pose3* Expmap_(Vector v);
Pose3* compose_(const Pose3& p2);
Pose3* between_(const Pose3& p2);
Pose3* retract_(Vector v);
Point3* translation_() const;
Rot3* rotation_() const;
};
class SharedGaussian {
@ -314,7 +296,7 @@ class PlanarSLAMOdometry {
class GaussianSequentialSolver {
GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR);
GaussianBayesNet* eliminate() const;
VectorValues* optimize() const; // FAIL: parse error here
VectorValues* optimize() const;
GaussianFactor* marginalFactor(int j) const;
Matrix marginalCovariance(int j) const;
};

18
gtsam/geometry/Point2.h
View File

@ -72,11 +72,6 @@ public:
return *this + p2;
}
/// MATLAB version returns shared pointer
boost::shared_ptr<Point2> compose_(const Point2& p2) {
return boost::shared_ptr<Point2>(new Point2(compose(p2)));
}
/** operators */
inline Point2 operator- () const {return Point2(-x_,-y_);}
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
@ -97,11 +92,6 @@ public:
/// Updates a with tangent space delta
inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
/// MATLAB version returns shared pointer
boost::shared_ptr<Point2> retract_(const Vector& v) {
return boost::shared_ptr<Point2>(new Point2(retract(v)));
}
/// Local coordinates of manifold neighborhood around current value
inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }
@ -111,9 +101,6 @@ public:
/// Exponential map around identity - just create a Point2 from a vector
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
static inline boost::shared_ptr<Point2> Expmap_(const Vector& v) {
return boost::shared_ptr<Point2>(new Point2(Expmap(v)));
}
/// Log map around identity - just return the Point2 as a vector
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
@ -149,11 +136,6 @@ public:
return p2 - (*this);
}
/// MATLAB version returns shared pointer
boost::shared_ptr<Point2> between_(const Point2& p2) {
return boost::shared_ptr<Point2>(new Point2(between(p2)));
}
/** get functions for x, y */
double x() const {return x_;}
double y() const {return y_;}

18
gtsam/geometry/Point3.h
View File

@ -76,11 +76,6 @@ namespace gtsam {
return *this + p2;
}
/// MATLAB version returns shared pointer
boost::shared_ptr<Point3> compose_(const Point3& p2) {
return boost::shared_ptr<Point3>(new Point3(compose(p2)));
}
/// @}
/// @name Manifold
/// @{
@ -94,11 +89,6 @@ namespace gtsam {
/// Updates a with tangent space delta
inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); }
/// MATLAB version returns shared pointer
boost::shared_ptr<Point3> retract_(const Vector& v) {
return boost::shared_ptr<Point3>(new Point3(retract(v)));
}
/// Returns inverse retraction
inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); }
@ -108,9 +98,6 @@ namespace gtsam {
/** Exponential map at identity - just create a Point3 from x,y,z */
static inline Point3 Expmap(const Vector& v) { return Point3(v); }
static inline boost::shared_ptr<Point3> Expmap_(const Vector& v) {
return boost::shared_ptr<Point3>(new Point3(Expmap(v)));
}
/** Log map at identity - return the x,y,z of this point */
static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
@ -151,11 +138,6 @@ namespace gtsam {
return p2 - *this;
}
/// MATLAB version returns shared pointer
boost::shared_ptr<Point3> between_(const Point3& p2) {
return boost::shared_ptr<Point3>(new Point3(between(p2)));
}
/** return vectorized form (column-wise)*/
Vector vector() const {
Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_;

18
gtsam/geometry/Pose2.h
View File

@ -103,11 +103,6 @@ public:
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose2> compose_(const Pose2& p2) {
return boost::shared_ptr<Pose2>(new Pose2(compose(p2)));
}
/// compose syntactic sugar
inline Pose2 operator*(const Pose2& p2) const {
return Pose2(r_*p2.r(), t_ + r_*p2.t());
@ -126,11 +121,6 @@ public:
/// Retraction from R^3 to Pose2 manifold neighborhood around current pose
Pose2 retract(const Vector& v) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose2> retract_(const Vector& v) {
return boost::shared_ptr<Pose2>(new Pose2(retract(v)));
}
/// Local 3D coordinates of Pose2 manifold neighborhood around current pose
Vector localCoordinates(const Pose2& p2) const;
@ -140,9 +130,6 @@ public:
/// Exponential map from Lie algebra se(2) to SE(2)
static Pose2 Expmap(const Vector& xi);
static inline boost::shared_ptr<Pose2> Expmap_(const Vector& v) {
return boost::shared_ptr<Pose2>(new Pose2(Expmap(v)));
}
/// Exponential map from SE(2) to Lie algebra se(2)
static Vector Logmap(const Pose2& p);
@ -159,11 +146,6 @@ public:
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose2> between_(const Pose2& p2) {
return boost::shared_ptr<Pose2>(new Pose2(between(p2)));
}
/**
* Return point coordinates in pose coordinate frame
*/

18
gtsam/geometry/Pose3.h
View File

@ -96,11 +96,6 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose3> compose_(const Pose3& p2) {
return boost::shared_ptr<Pose3>(new Pose3(compose(p2)));
}
/// compose syntactic sugar
Pose3 operator*(const Pose3& T) const {
return Pose3(R_*T.R_, t_ + R_*T.t_);
@ -120,11 +115,6 @@ namespace gtsam {
/// Retraction from R^6 to Pose3 manifold neighborhood around current pose
Pose3 retract(const Vector& d) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose3> retract_(const Vector& v) {
return boost::shared_ptr<Pose3>(new Pose3(retract(v)));
}
/// Local 6D coordinates of Pose3 manifold neighborhood around current pose
Vector localCoordinates(const Pose3& T2) const;
@ -134,9 +124,6 @@ namespace gtsam {
/// Exponential map from Lie algebra se(3) to SE(3)
static Pose3 Expmap(const Vector& xi);
static inline boost::shared_ptr<Pose3> Expmap_(const Vector& v) {
return boost::shared_ptr<Pose3>(new Pose3(Expmap(v)));
}
/// Exponential map from SE(3) to Lie algebra se(3)
static Vector Logmap(const Pose3& p);
@ -164,11 +151,6 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose3> between_(const Pose3& p2) {
return boost::shared_ptr<Pose3>(new Pose3(between(p2)));
}
/**
* Calculate Adjoint map
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)

18
gtsam/geometry/Rot2.h
View File

@ -138,11 +138,6 @@ namespace gtsam {
return *this * R1;
}
/// MATLAB version returns shared pointer
boost::shared_ptr<Rot2> compose_(const Rot2& p2) {
return boost::shared_ptr<Rot2>(new Rot2(compose(p2)));
}
/** Compose - make a new rotation by adding angles */
Rot2 operator*(const Rot2& R) const {
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
@ -170,11 +165,6 @@ namespace gtsam {
/// Updates a with tangent space delta
inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
/// MATLAB version returns shared pointer
boost::shared_ptr<Rot2> retract_(const Vector& v) {
return boost::shared_ptr<Rot2>(new Rot2(retract(v)));
}
/// Returns inverse retraction
inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
@ -189,9 +179,6 @@ namespace gtsam {
else
return Rot2::fromAngle(v(0));
}
static inline boost::shared_ptr<Rot2> Expmap_(const Vector& v) {
return boost::shared_ptr<Rot2>(new Rot2(Expmap(v)));
}
/// Logmap around identity - return the angle of the rotation
static inline Vector Logmap(const Rot2& r) {
@ -219,11 +206,6 @@ namespace gtsam {
return between_default(*this, p2);
}
/// MATLAB version returns shared pointer
boost::shared_ptr<Rot2> between_(const Rot2& p2) {
return boost::shared_ptr<Rot2>(new Rot2(between(p2)));
}
/** return 2*2 rotation matrix */
Matrix matrix() const;

18
gtsam/geometry/Rot3M.h
View File

@ -152,11 +152,6 @@ namespace gtsam {
Rot3M compose(const Rot3M& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Rot3M> compose_(const Rot3M& p2) {
return boost::shared_ptr<Rot3M>(new Rot3M(compose(p2)));
}
/// rotate point from rotated coordinate frame to world = R*p
inline Point3 operator*(const Point3& p) const { return rotate(p);}
@ -182,11 +177,6 @@ namespace gtsam {
/// Updates a with tangent space delta
Rot3M retract(const Vector& v) const { return compose(Expmap(v)); }
/// MATLAB version returns shared pointer
boost::shared_ptr<Rot3M> retract_(const Vector& v) {
return boost::shared_ptr<Rot3M>(new Rot3M(retract(v)));
}
/// Returns inverse retraction
Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); }
@ -202,9 +192,6 @@ namespace gtsam {
if(zero(v)) return Rot3M();
else return rodriguez(v);
}
static inline boost::shared_ptr<Rot3M> Expmap_(const Vector& v) {
return boost::shared_ptr<Rot3M>(new Rot3M(Expmap(v)));
}
/**
* Log map at identity - return the canonical coordinates of this rotation
@ -260,11 +247,6 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Rot3M> between_(const Rot3M& p2) {
return boost::shared_ptr<Rot3M>(new Rot3M(between(p2)));
}
/** compose two rotations */
Rot3M operator*(const Rot3M& R2) const {
return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));