Indentation
Richard Roberts
parent
a7a5e5e816
commit
94f986bbe7
106
cpp/Pose2.cpp
106
cpp/Pose2.cpp
|
|
@ -9,68 +9,68 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Pose2::print(const string& s) const {
|
||||
cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void Pose2::print(const string& s) const {
|
||||
cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Pose2::equals(const Pose2& q, double tol) const {
|
||||
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
bool Pose2::equals(const Pose2& q, double tol) const {
|
||||
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::exmap(const Vector& v) const {
|
||||
return Pose2(t_+Point2(v(0),v(1)), r_.exmap(Vector_(1, v(2))));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::exmap(const Vector& v) const {
|
||||
return Pose2(t_+Point2(v(0),v(1)), r_.exmap(Vector_(1, v(2))));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose2::vector() const {
|
||||
return Vector_(3, t_.x(), t_.y(), r_.theta());
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Vector Pose2::vector() const {
|
||||
return Vector_(3, t_.x(), t_.y(), r_.theta());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Pose2 Pose2::rotate(double theta) const {
|
||||
// //return Pose2(t_, Rot2(theta)*r_);
|
||||
// return Pose2(Point2(0.0,0.0),-theta)*(*this);
|
||||
// }
|
||||
/* ************************************************************************* */
|
||||
// Pose2 Pose2::rotate(double theta) const {
|
||||
// //return Pose2(t_, Rot2(theta)*r_);
|
||||
// return Pose2(Point2(0.0,0.0),-theta)*(*this);
|
||||
// }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 transform_to(const Pose2& pose, const Point2& point) {
|
||||
return pose*point;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point2 transform_to(const Pose2& pose, const Point2& point) {
|
||||
return pose*point;
|
||||
}
|
||||
|
||||
// TODO, have a combined function that returns both function value and derivative
|
||||
Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
|
||||
Matrix dx_dt = pose.r().negtranspose();
|
||||
Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t());
|
||||
return collect(2, &dx_dt, &dx_dr);
|
||||
}
|
||||
// TODO, have a combined function that returns both function value and derivative
|
||||
Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
|
||||
Matrix dx_dt = pose.r().negtranspose();
|
||||
Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t());
|
||||
return collect(2, &dx_dt, &dx_dr);
|
||||
}
|
||||
|
||||
Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
|
||||
return pose.r().transpose();
|
||||
}
|
||||
Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
|
||||
return pose.r().transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 between(const Pose2& p1, const Pose2& p2) {
|
||||
return Pose2(
|
||||
p1.r().invcompose(p2.r()),
|
||||
p1.r().unrotate(p2.t() - p1.t()));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Pose2 between(const Pose2& p1, const Pose2& p2) {
|
||||
return Pose2(
|
||||
p1.r().invcompose(p2.r()),
|
||||
p1.r().unrotate(p2.t() - p1.t()));
|
||||
}
|
||||
|
||||
Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
|
||||
Matrix dbt_dp = Dtransform_to1(p1, p2.t());
|
||||
Matrix dbr_dp = Matrix_(1,3, 0.0, 0.0, -1.0);
|
||||
return stack(2, &dbt_dp, &dbr_dp);
|
||||
}
|
||||
Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
|
||||
Matrix dbt_dp = Dtransform_to1(p1, p2.t());
|
||||
Matrix dbr_dp = Matrix_(1,3, 0.0, 0.0, -1.0);
|
||||
return stack(2, &dbt_dp, &dbr_dp);
|
||||
}
|
||||
|
||||
Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
|
||||
Matrix db_dt2 = p1.r().transpose();
|
||||
return Matrix_(3,3,
|
||||
db_dt2.data()[0], db_dt2.data()[1], 0.0,
|
||||
db_dt2.data()[2], db_dt2.data()[3], 0.0,
|
||||
0.0, 0.0, 1.0);
|
||||
}
|
||||
Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
|
||||
Matrix db_dt2 = p1.r().transpose();
|
||||
return Matrix_(3,3,
|
||||
db_dt2.data()[0], db_dt2.data()[1], 0.0,
|
||||
db_dt2.data()[2], db_dt2.data()[3], 0.0,
|
||||
0.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
196
cpp/Pose2.h
196
cpp/Pose2.h
|
|
@ -16,111 +16,111 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A 2D pose (Point2,Rot2)
|
||||
*/
|
||||
class Pose2: Testable<Pose2> {
|
||||
/**
|
||||
* A 2D pose (Point2,Rot2)
|
||||
*/
|
||||
class Pose2: Testable<Pose2> {
|
||||
|
||||
private:
|
||||
Point2 t_;
|
||||
Rot2 r_;
|
||||
private:
|
||||
Point2 t_;
|
||||
Rot2 r_;
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/** default constructor = origin */
|
||||
Pose2() :
|
||||
t_(0.0, 0.0), r_(0) { } // default is origin
|
||||
/** default constructor = origin */
|
||||
Pose2() :
|
||||
t_(0.0, 0.0), r_(0) { } // default is origin
|
||||
|
||||
/** copy constructor */
|
||||
Pose2(const Pose2& pose) :
|
||||
t_(pose.t_), r_(pose.r_) { }
|
||||
/** copy constructor */
|
||||
Pose2(const Pose2& pose) :
|
||||
t_(pose.t_), r_(pose.r_) { }
|
||||
|
||||
/**
|
||||
* construct from (x,y,theta)
|
||||
* @param x x coordinate
|
||||
* @param y y coordinate
|
||||
* @param theta angle with positive X-axis
|
||||
*/
|
||||
Pose2(double x, double y, double theta) :
|
||||
t_(x,y), r_(theta) { }
|
||||
|
||||
/** construct from rotation and translation */
|
||||
Pose2(const Point2& t, double theta) :
|
||||
t_(t), r_(theta) { }
|
||||
Pose2(double theta, const Point2& t) :
|
||||
t_(t), r_(theta) { }
|
||||
Pose2(const Point2& t, const Rot2& r) :
|
||||
t_(t), r_(r) { }
|
||||
Pose2(const Rot2& r, const Point2& t) :
|
||||
t_(t), r_(r) { }
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** assert equality up to a tolerance */
|
||||
bool equals(const Pose2& pose, double tol = 1e-9) const;
|
||||
|
||||
/** get functions for x, y, theta */
|
||||
double x() const { return t_.x();}
|
||||
double y() const { return t_.y();}
|
||||
double theta() const { return r_.theta();}
|
||||
Point2 t() const { return t_; }
|
||||
Rot2 r() const { return r_; }
|
||||
|
||||
/** return DOF, dimensionality of tangent space = 3 */
|
||||
size_t dim() const { return 3; }
|
||||
|
||||
/* exponential map */
|
||||
Pose2 exmap(const Vector& v) const;
|
||||
|
||||
/** return vectorized form (column-wise) */
|
||||
Vector vector() const;
|
||||
|
||||
/** rotate pose by theta */
|
||||
// Pose2 rotate(double theta) const;
|
||||
|
||||
/** inverse transformation */
|
||||
Pose2 inverse() const {
|
||||
return Pose2(r_, r_.unrotate(t_));
|
||||
}
|
||||
|
||||
/** compose this transformation onto another (pre-multiply this*p1) */
|
||||
Pose2 compose(const Pose2& p1) const {
|
||||
return Pose2(p1.r_*r_, p1.r_*t_+p1.t_);
|
||||
}
|
||||
|
||||
/** same as compose (pre-multiply this*p1) */
|
||||
Pose2 operator*(const Pose2& p1) const {
|
||||
return compose(p1);
|
||||
}
|
||||
|
||||
/** Return point coordinates in pose coordinate frame, same as transform_to */
|
||||
Point2 operator*(const Point2& point) const {
|
||||
return r_.unrotate(point-t_);
|
||||
}
|
||||
|
||||
// operators
|
||||
Pose2 operator+(const Pose2& p2) const {
|
||||
return Pose2(t_+p2.t_, r_*p2.r_);
|
||||
}
|
||||
|
||||
Pose2 operator-(const Pose2& p2) const {
|
||||
return Pose2(t_-p2.t_, r_.invcompose(p2.r_));
|
||||
}
|
||||
}; // Pose2
|
||||
|
||||
/**
|
||||
* construct from (x,y,theta)
|
||||
* @param x x coordinate
|
||||
* @param y y coordinate
|
||||
* @param theta angle with positive X-axis
|
||||
* Return point coordinates in pose coordinate frame
|
||||
*/
|
||||
Pose2(double x, double y, double theta) :
|
||||
t_(x,y), r_(theta) { }
|
||||
Point2 transform_to(const Pose2& pose, const Point2& point);
|
||||
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
|
||||
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
|
||||
|
||||
/** construct from rotation and translation */
|
||||
Pose2(const Point2& t, double theta) :
|
||||
t_(t), r_(theta) { }
|
||||
Pose2(double theta, const Point2& t) :
|
||||
t_(t), r_(theta) { }
|
||||
Pose2(const Point2& t, const Rot2& r) :
|
||||
t_(t), r_(r) { }
|
||||
Pose2(const Rot2& r, const Point2& t) :
|
||||
t_(t), r_(r) { }
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** assert equality up to a tolerance */
|
||||
bool equals(const Pose2& pose, double tol = 1e-9) const;
|
||||
|
||||
/** get functions for x, y, theta */
|
||||
double x() const { return t_.x();}
|
||||
double y() const { return t_.y();}
|
||||
double theta() const { return r_.theta();}
|
||||
Point2 t() const { return t_; }
|
||||
Rot2 r() const { return r_; }
|
||||
|
||||
/** return DOF, dimensionality of tangent space = 3 */
|
||||
size_t dim() const { return 3; }
|
||||
|
||||
/* exponential map */
|
||||
Pose2 exmap(const Vector& v) const;
|
||||
|
||||
/** return vectorized form (column-wise) */
|
||||
Vector vector() const;
|
||||
|
||||
/** rotate pose by theta */
|
||||
// Pose2 rotate(double theta) const;
|
||||
|
||||
/** inverse transformation */
|
||||
Pose2 inverse() const {
|
||||
return Pose2(r_, r_.unrotate(t_));
|
||||
}
|
||||
|
||||
/** compose this transformation onto another (pre-multiply this*p1) */
|
||||
Pose2 compose(const Pose2& p1) const {
|
||||
return Pose2(p1.r_*r_, p1.r_*t_+p1.t_);
|
||||
}
|
||||
|
||||
/** same as compose (pre-multiply this*p1) */
|
||||
Pose2 operator*(const Pose2& p1) const {
|
||||
return compose(p1);
|
||||
}
|
||||
|
||||
/** Return point coordinates in pose coordinate frame, same as transform_to */
|
||||
Point2 operator*(const Point2& point) const {
|
||||
return r_.unrotate(point-t_);
|
||||
}
|
||||
|
||||
// operators
|
||||
Pose2 operator+(const Pose2& p2) const {
|
||||
return Pose2(t_+p2.t_, r_*p2.r_);
|
||||
}
|
||||
|
||||
Pose2 operator-(const Pose2& p2) const {
|
||||
return Pose2(t_-p2.t_, r_.invcompose(p2.r_));
|
||||
}
|
||||
}; // Pose2
|
||||
|
||||
/**
|
||||
* Return point coordinates in pose coordinate frame
|
||||
*/
|
||||
Point2 transform_to(const Pose2& pose, const Point2& point);
|
||||
Matrix Dtransform_to1(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
|
||||
*/
|
||||
Pose2 between(const Pose2& p1, const Pose2& p2);
|
||||
Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
|
||||
Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
|
||||
/**
|
||||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
*/
|
||||
Pose2 between(const Pose2& p1, const Pose2& p2);
|
||||
Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
|
||||
Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
174
cpp/Rot2.cpp
174
cpp/Rot2.cpp
|
|
@ -11,109 +11,109 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Rot2::print(const string& s) const {
|
||||
cout << s << ":" << angle() << endl;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void Rot2::print(const string& s) const {
|
||||
cout << s << ":" << angle() << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot2::equals(const Rot2& R, double tol) const {
|
||||
return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
bool Rot2::equals(const Rot2& R, double tol) const {
|
||||
return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::exmap(const Vector& v) const {
|
||||
if (zero(v)) return (*this);
|
||||
return Rot2(v(0)) * (*this); // exponential map then compose
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::exmap(const Vector& v) const {
|
||||
if (zero(v)) return (*this);
|
||||
return Rot2(v(0)) * (*this); // exponential map then compose
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot2::matrix() const {
|
||||
return Matrix_(2, 2, c_, -s_, s_, c_);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot2::matrix() const {
|
||||
return Matrix_(2, 2, c_, -s_, s_, c_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot2::transpose() const {
|
||||
return Matrix_(2, 2, c_, s_, -s_, c_);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot2::transpose() const {
|
||||
return Matrix_(2, 2, c_, s_, -s_, c_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot2::negtranspose() const {
|
||||
return Matrix_(2, 2, -c_, -s_, s_, -c_);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot2::negtranspose() const {
|
||||
return Matrix_(2, 2, -c_, -s_, s_, -c_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::inverse() const { return Rot2(c_, -s_);}
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::inverse() const { return Rot2(c_, -s_);}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::invcompose(const Rot2& R) const {
|
||||
return Rot2(
|
||||
c_ * R.c_ + s_ * R.s_,
|
||||
-s_ * R.c_ + c_ * R.s_);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::invcompose(const Rot2& R) const {
|
||||
return Rot2(
|
||||
c_ * R.c_ + s_ * R.s_,
|
||||
-s_ * R.c_ + c_ * R.s_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::operator*(const Rot2& R) const {
|
||||
return Rot2(
|
||||
c_ * R.c_ - s_ * R.s_,
|
||||
s_ * R.c_ + c_ * R.s_
|
||||
);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::operator*(const Rot2& R) const {
|
||||
return Rot2(
|
||||
c_ * R.c_ - s_ * R.s_,
|
||||
s_ * R.c_ + c_ * R.s_
|
||||
);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Rot2::operator*(const Point2& p) const {
|
||||
return Point2(
|
||||
c_ * p.x() - s_ * p.y(),
|
||||
s_ * p.x() + c_ * p.y()
|
||||
);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point2 Rot2::operator*(const Point2& p) const {
|
||||
return Point2(
|
||||
c_ * p.x() - s_ * p.y(),
|
||||
s_ * p.x() + c_ * p.y()
|
||||
);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Rot2::unrotate(const Point2& p) const {
|
||||
return Point2(
|
||||
c_ * p.x() + s_ * p.y(),
|
||||
-s_ * p.x() + c_ * p.y()
|
||||
);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point2 Rot2::unrotate(const Point2& p) const {
|
||||
return Point2(
|
||||
c_ * p.x() + s_ * p.y(),
|
||||
-s_ * p.x() + c_ * p.y()
|
||||
);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 exmap(const Rot2& R, const Vector& v) {
|
||||
return R.exmap(v);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot2 exmap(const Rot2& R, const Vector& v) {
|
||||
return R.exmap(v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 rotate(const Rot2& R, const Point2& p) {
|
||||
return R * p;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point2 rotate(const Rot2& R, const Point2& p) {
|
||||
return R * p;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see libraries/caml/geometry/math.ml
|
||||
Matrix Drotate1(const Rot2& R, const Point2& p) {
|
||||
Point2 q = R*p;
|
||||
return Matrix_(2, 1, -q.y(), q.x());
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
// see libraries/caml/geometry/math.ml
|
||||
Matrix Drotate1(const Rot2& R, const Point2& p) {
|
||||
Point2 q = R*p;
|
||||
return Matrix_(2, 1, -q.y(), q.x());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Drotate2(const Rot2& R) {
|
||||
return R.matrix();
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Matrix Drotate2(const Rot2& R) {
|
||||
return R.matrix();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 unrotate(const Rot2& R, const Point2& p) {
|
||||
return R.unrotate(p);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point2 unrotate(const Rot2& R, const Point2& p) {
|
||||
return R.unrotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** see libraries/caml/geometry/math.lyx, derivative of unrotate */
|
||||
/* ************************************************************************* */
|
||||
Matrix Dunrotate1(const Rot2 & R, const Point2 & p) {
|
||||
Point2 q = R.unrotate(p);
|
||||
return Matrix_(2, 1, q.y(), -q.x());
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
/** see libraries/caml/geometry/math.lyx, derivative of unrotate */
|
||||
/* ************************************************************************* */
|
||||
Matrix Dunrotate1(const Rot2 & R, const Point2 & p) {
|
||||
Point2 q = R.unrotate(p);
|
||||
return Matrix_(2, 1, q.y(), -q.x());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Dunrotate2(const Rot2 & R) {
|
||||
return R.transpose();
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Matrix Dunrotate2(const Rot2 & R) {
|
||||
return R.transpose();
|
||||
}
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
158
cpp/Rot2.h
158
cpp/Rot2.h
|
|
@ -14,110 +14,110 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* Rotation matrix */
|
||||
class Rot2: Testable<Rot2> {
|
||||
private:
|
||||
/** we store cos(theta) and sin(theta) */
|
||||
double c_, s_;
|
||||
/* Rotation matrix */
|
||||
class Rot2: Testable<Rot2> {
|
||||
private:
|
||||
/** we store cos(theta) and sin(theta) */
|
||||
double c_, s_;
|
||||
|
||||
/** private constructor from cos/sin */
|
||||
Rot2(double c, double s) :
|
||||
c_(c), s_(s) {
|
||||
}
|
||||
/** private constructor from cos/sin */
|
||||
Rot2(double c, double s) :
|
||||
c_(c), s_(s) {
|
||||
}
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/** default constructor, zero rotation */
|
||||
Rot2() : c_(1.0), s_(0.0) {}
|
||||
/** default constructor, zero rotation */
|
||||
Rot2() : c_(1.0), s_(0.0) {}
|
||||
|
||||
/** constructor from angle == exponential map at identity */
|
||||
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
|
||||
/** constructor from angle == exponential map at identity */
|
||||
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
|
||||
|
||||
/** return angle */
|
||||
double theta() const { return atan2(s_,c_); }
|
||||
double angle() const { return atan2(s_,c_); }
|
||||
/** return angle */
|
||||
double theta() const { return atan2(s_,c_); }
|
||||
double angle() const { return atan2(s_,c_); }
|
||||
|
||||
/** return cos */
|
||||
double c() const { return c_; }
|
||||
/** return cos */
|
||||
double c() const { return c_; }
|
||||
|
||||
/** return sin */
|
||||
double s() const { return s_; }
|
||||
/** return sin */
|
||||
double s() const { return s_; }
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "theta") const;
|
||||
/** print */
|
||||
void print(const std::string& s = "theta") const;
|
||||
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Rot2& R, double tol = 1e-9) const;
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Rot2& R, double tol = 1e-9) const;
|
||||
|
||||
/** return DOF, dimensionality of tangent space */
|
||||
inline size_t dim() const { return 1;}
|
||||
/** return DOF, dimensionality of tangent space */
|
||||
inline size_t dim() const { return 1;}
|
||||
|
||||
/** Given 1-dim tangent vector, create new rotation */
|
||||
Rot2 exmap(const Vector& d) const;
|
||||
/** Given 1-dim tangent vector, create new rotation */
|
||||
Rot2 exmap(const Vector& d) const;
|
||||
|
||||
/** return vectorized form (column-wise)*/
|
||||
inline Vector vector() const { return Vector_(2,c_,s_);}
|
||||
/** return vectorized form (column-wise)*/
|
||||
inline Vector vector() const { return Vector_(2,c_,s_);}
|
||||
|
||||
/** return 2*2 rotation matrix */
|
||||
Matrix matrix() const;
|
||||
/** return 2*2 rotation matrix */
|
||||
Matrix matrix() const;
|
||||
|
||||
/** return 2*2 transpose (inverse) rotation matrix */
|
||||
Matrix transpose() const;
|
||||
/** return 2*2 transpose (inverse) rotation matrix */
|
||||
Matrix transpose() const;
|
||||
|
||||
/** return 2*2 negative transpose */
|
||||
Matrix negtranspose() const;
|
||||
/** return 2*2 negative transpose */
|
||||
Matrix negtranspose() const;
|
||||
|
||||
/** inverse transformation */
|
||||
Rot2 inverse() const;
|
||||
/** inverse transformation */
|
||||
Rot2 inverse() const;
|
||||
|
||||
/** compose with the inverse of this rotation */
|
||||
Rot2 invcompose(const Rot2& R) const;
|
||||
/** compose with the inverse of this rotation */
|
||||
Rot2 invcompose(const Rot2& R) const;
|
||||
|
||||
/** composition via sum and difference formulas */
|
||||
Rot2 operator*(const Rot2& R) const;
|
||||
/** composition via sum and difference formulas */
|
||||
Rot2 operator*(const Rot2& R) const;
|
||||
|
||||
/** rotate from rotated to world, syntactic sugar = R*p */
|
||||
Point2 operator*(const Point2& p) const;
|
||||
/** rotate from rotated to world, syntactic sugar = R*p */
|
||||
Point2 operator*(const Point2& p) const;
|
||||
|
||||
/** rotate from world to rotated = R'*p */
|
||||
Point2 unrotate(const Point2& p) const;
|
||||
/** rotate from world to rotated = R'*p */
|
||||
Point2 unrotate(const Point2& p) const;
|
||||
|
||||
/** friends */
|
||||
friend Matrix Dunrotate1(const Rot2& R, const Point2& p);
|
||||
/** friends */
|
||||
friend Matrix Dunrotate1(const Rot2& R, const Point2& p);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(c_);
|
||||
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||
}
|
||||
};
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(c_);
|
||||
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Update Rotation with incremental rotation
|
||||
* @param v a vector of incremental angle
|
||||
* @param R a 2D rotation
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
Rot2 exmap(const Rot2& R, const Vector& v);
|
||||
/**
|
||||
* Update Rotation with incremental rotation
|
||||
* @param v a vector of incremental angle
|
||||
* @param R a 2D rotation
|
||||
* @return incremental rotation matrix
|
||||
*/
|
||||
Rot2 exmap(const Rot2& R, const Vector& v);
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
*/
|
||||
Point2 rotate(const Rot2& R, const Point2& p);
|
||||
Matrix Drotate1(const Rot2& R, const Point2& p);
|
||||
Matrix Drotate2(const Rot2& R); // does not depend on p !
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
*/
|
||||
Point2 rotate(const Rot2& R, const Point2& p);
|
||||
Matrix Drotate1(const Rot2& R, const Point2& p);
|
||||
Matrix Drotate2(const Rot2& R); // does not depend on p !
|
||||
|
||||
/**
|
||||
* rotate point from world to rotated
|
||||
* frame = R'*p
|
||||
*/
|
||||
Point2 unrotate(const Rot2& R, const Point2& p);
|
||||
Matrix Dunrotate1(const Rot2& R, const Point2& p);
|
||||
Matrix Dunrotate2(const Rot2& R); // does not depend on p !
|
||||
/**
|
||||
* rotate point from world to rotated
|
||||
* frame = R'*p
|
||||
*/
|
||||
Point2 unrotate(const Rot2& R, const Point2& p);
|
||||
Matrix Dunrotate1(const Rot2& R, const Point2& p);
|
||||
Matrix Dunrotate2(const Rot2& R); // does not depend on p !
|
||||
|
||||
} // gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -12,12 +12,18 @@
|
|||
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TEST(Pose2, print) {
|
||||
// Pose2 pose(Point2(1.0,2.0), Rot2(3.0));
|
||||
// pose.print("thepose");
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, constructors) {
|
||||
Point2 p;
|
||||
Pose2 pose(p,0);
|
||||
Pose2 origin;
|
||||
assert_equal(pose,origin);
|
||||
Point2 p;
|
||||
Pose2 pose(p,0);
|
||||
Pose2 origin;
|
||||
assert_equal(pose,origin);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -39,35 +45,35 @@ TEST(Pose2, exmap) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, operators) {
|
||||
CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1)));
|
||||
CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1)));
|
||||
CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1)));
|
||||
CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose2, transform_to )
|
||||
{
|
||||
Pose2 pose(1,2,M_PI_2); // robot at (1,2) looking towards y
|
||||
Point2 point(-1,4); // landmark at (-1,4)
|
||||
Pose2 pose(1,2,M_PI_2); // robot at (1,2) looking towards y
|
||||
Point2 point(-1,4); // landmark at (-1,4)
|
||||
|
||||
// expected
|
||||
Point2 expected(2,2);
|
||||
Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0);
|
||||
Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0);
|
||||
// expected
|
||||
Point2 expected(2,2);
|
||||
Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0);
|
||||
Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0);
|
||||
|
||||
// actual
|
||||
Point2 actual = transform_to(pose,point);
|
||||
Matrix actualH1 = Dtransform_to1(pose,point);
|
||||
Matrix actualH2 = Dtransform_to2(pose,point);
|
||||
// actual
|
||||
Point2 actual = transform_to(pose,point);
|
||||
Matrix actualH1 = Dtransform_to1(pose,point);
|
||||
Matrix actualH2 = Dtransform_to2(pose,point);
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
CHECK(assert_equal(expectedH2,actualH2));
|
||||
CHECK(assert_equal(expected,actual));
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
CHECK(assert_equal(expectedH2,actualH2));
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5);
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5);
|
||||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -104,42 +110,42 @@ TEST(Pose2, compose_b)
|
|||
/* ************************************************************************* */
|
||||
TEST( Pose2, between )
|
||||
{
|
||||
Pose2 p1(1,2,M_PI_2); // robot at (1,2) looking towards y
|
||||
Pose2 p2(-1,4,M_PI); // robot at (-1,4) loooking at negative x
|
||||
Pose2 p1(1,2,M_PI_2); // robot at (1,2) looking towards y
|
||||
Pose2 p2(-1,4,M_PI); // robot at (-1,4) loooking at negative x
|
||||
|
||||
// expected
|
||||
Pose2 expected(2,2,M_PI_2);
|
||||
Matrix expectedH1 = Matrix_(3,3,
|
||||
0.0,-1.0,2.0,
|
||||
1.0,0.0,-2.0,
|
||||
0.0,0.0,-1.0
|
||||
);
|
||||
Matrix expectedH2 = Matrix_(3,3,
|
||||
0.0,1.0,0.0,
|
||||
-1.0,0.0,0.0,
|
||||
0.0,0.0,1.0
|
||||
);
|
||||
// expected
|
||||
Pose2 expected(2,2,M_PI_2);
|
||||
Matrix expectedH1 = Matrix_(3,3,
|
||||
0.0,-1.0,2.0,
|
||||
1.0,0.0,-2.0,
|
||||
0.0,0.0,-1.0
|
||||
);
|
||||
Matrix expectedH2 = Matrix_(3,3,
|
||||
0.0,1.0,0.0,
|
||||
-1.0,0.0,0.0,
|
||||
0.0,0.0,1.0
|
||||
);
|
||||
|
||||
// actual
|
||||
Pose2 actual = between(p1,p2);
|
||||
Matrix actualH1 = Dbetween1(p1,p2);
|
||||
Matrix actualH2 = Dbetween2(p1,p2);
|
||||
// actual
|
||||
Pose2 actual = between(p1,p2);
|
||||
Matrix actualH1 = Dbetween1(p1,p2);
|
||||
Matrix actualH2 = Dbetween2(p1,p2);
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
CHECK(assert_equal(expectedH2,actualH2));
|
||||
CHECK(assert_equal(expected,actual));
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
CHECK(assert_equal(expectedH2,actualH2));
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5);
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5);
|
||||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue