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

added retract_ function for matlab wrapper of Pose2

release/4.3a0
Stephen Williams 2011-11-29 19:02:36 +00:00
parent 9fcf789e3d
commit ca121c2872
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam.h
View File

@ -41,6 +41,7 @@ class Pose2 {
Pose2* compose_(const Pose2& p2); Pose2* compose_(const Pose2& p2);
Pose2* between_(const Pose2& p2); Pose2* between_(const Pose2& p2);
Vector localCoordinates(const Pose2& p); Vector localCoordinates(const Pose2& p);
Pose2* retract_(Vector v);
}; };
class SharedGaussian { class SharedGaussian {
@ -181,6 +182,8 @@ class PlanarSLAMOdometry {
class GaussianSequentialSolver { class GaussianSequentialSolver {
GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR);
GaussianBayesNet* eliminate(); GaussianBayesNet* eliminate() const;
VectorValues* optimize(); VectorValues* optimize() const;
GaussianFactor* marginalFactor(int j) const;
Matrix marginalCovariance(int j) const;
}; };

5
gtsam/geometry/Pose2.h
View File

@ -126,6 +126,11 @@ public:
/// Retraction from R^3 to Pose2 manifold neighborhood around current pose /// Retraction from R^3 to Pose2 manifold neighborhood around current pose
Pose2 retract(const Vector& v) const; 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 /// Local 3D coordinates of Pose2 manifold neighborhood around current pose
Vector localCoordinates(const Pose2& p2) const; Vector localCoordinates(const Pose2& p2) const;