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

Indentation

release/4.3a0
Richard Roberts 2009-12-15 00:00:02 +00:00
parent a7a5e5e816
commit 94f986bbe7
5 changed files with 369 additions and 363 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cpp/Pose2.cpp
View File

@ -30,10 +30,10 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Pose2 Pose2::rotate(double theta) const { // Pose2 Pose2::rotate(double theta) const {
// //return Pose2(t_, Rot2(theta)*r_); // //return Pose2(t_, Rot2(theta)*r_);
// return Pose2(Point2(0.0,0.0),-theta)*(*this); // return Pose2(Point2(0.0,0.0),-theta)*(*this);
// } // }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 transform_to(const Pose2& pose, const Point2& point) { Point2 transform_to(const Pose2& pose, const Point2& point) {

28
cpp/Pose2.h
View File

@ -16,16 +16,16 @@
namespace gtsam { namespace gtsam {
/** /**
* A 2D pose (Point2,Rot2) * A 2D pose (Point2,Rot2)
*/ */
class Pose2: Testable<Pose2> { class Pose2: Testable<Pose2> {
private: private:
Point2 t_; Point2 t_;
Rot2 r_; Rot2 r_;
public: public:
/** default constructor = origin */ /** default constructor = origin */
Pose2() : Pose2() :
@ -77,7 +77,7 @@ public:
Vector vector() const; Vector vector() const;
/** rotate pose by theta */ /** rotate pose by theta */
// Pose2 rotate(double theta) const; // Pose2 rotate(double theta) const;
/** inverse transformation */ /** inverse transformation */
Pose2 inverse() const { Pose2 inverse() const {
@ -107,20 +107,20 @@ public:
Pose2 operator-(const Pose2& p2) const { Pose2 operator-(const Pose2& p2) const {
return Pose2(t_-p2.t_, r_.invcompose(p2.r_)); return Pose2(t_-p2.t_, r_.invcompose(p2.r_));
} }
}; // Pose2 }; // Pose2
/** /**
* Return point coordinates in pose coordinate frame * Return point coordinates in pose coordinate frame
*/ */
Point2 transform_to(const Pose2& pose, const Point2& point); Point2 transform_to(const Pose2& pose, const Point2& point);
Matrix Dtransform_to1(const Pose2& pose, const Point2& point); Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
Matrix Dtransform_to2(const Pose2& pose, const Point2& point); Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
*/ */
Pose2 between(const Pose2& p1, const Pose2& p2); Pose2 between(const Pose2& p1, const Pose2& p2);
Matrix Dbetween1(const Pose2& p1, const Pose2& p2); Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
Matrix Dbetween2(const Pose2& p1, const Pose2& p2); Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
} // namespace gtsam } // namespace gtsam

6
cpp/testPose2.cpp
View File

@ -12,6 +12,12 @@
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */
//TEST(Pose2, print) {
// Pose2 pose(Point2(1.0,2.0), Rot2(3.0));
// pose.print("thepose");
//}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, constructors) { TEST(Pose2, constructors) {
Point2 p; Point2 p;