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

Wrapped Expmap, logmap, Expmap constructor

release/4.3a0
Frank Dellaert 2011-11-04 06:10:20 +00:00
parent d54cc85336
commit 847e0cb3e4
3 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam.h
View File

@ -28,10 +28,10 @@ class Rot2 {
class Pose2 {
Pose2();
Pose2(const Pose2& pose);
Pose2(double x, double y, double theta);
Pose2(double theta, const Point2& t);
Pose2(const Rot2& r, const Point2& t);
Pose2(Vector v);
void print(string s) const;
bool equals(const Pose2& pose, double tol) const;
double x() const;
@ -40,11 +40,11 @@ class Pose2 {
int dim() const;
Pose2* compose_(const Pose2& p2);
Pose2* between_(const Pose2& p2);
Vector logmap(const Pose2& p);
};
class SharedGaussian {
SharedGaussian(Matrix covariance);
SharedGaussian(Vector sigmas);
};
class SharedDiagonal {

11
gtsam/geometry/Pose2.h
View File

@ -70,7 +70,16 @@ namespace gtsam {
/** Constructor from 3*3 matrix */
Pose2(const Matrix &T) :
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { assert(T.rows()==3 && T.cols()==3); }
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
assert(T.rows() == 3 && T.cols() == 3);
}
/** Construct from canonical coordinates (Lie algebra) */
Pose2(const Vector& v) {
Pose2 pose = Expmap(v);
r_ = pose.r_;
t_ = pose.t_;
}
/** print with optional string */
void print(const std::string& s = "") const;

5
gtsam/geometry/tests/testPose2.cpp
View File

@ -103,7 +103,10 @@ TEST(Pose2, expmap2) {
Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
Matrix expected = eye(3) + A + A2 + A3 + A4;
Pose2 pose = Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.99));
Vector v = Vector_(3, 0.01, -0.015, 0.99);
Pose2 pose = Pose2::Expmap(v);
Pose2 pose2(v);
EXPECT(assert_equal(pose, pose2));
Matrix actual = pose.matrix();
//EXPECT(assert_equal(expected, actual));
}