Closing # 118: Made Pose2 use Rot2, added 'compose' for Pose2, made slight modifications to Rot2. Also added additional unit tests. This change shouldn't break anything but let me know if it does.
Richard Roberts
parent
0116b3a515
commit
c4b60bfd65
|
|
@ -30,8 +30,8 @@ namespace gtsam {
|
|||
/** print with optional string */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** equals with an tolerance, prints out message if unequal*/
|
||||
bool equals(const Point2& q, double tol = 1e-9) const;
|
||||
/** equals with an tolerance, prints out message if unequal*/
|
||||
bool equals(const Point2& q, double tol = 1e-9) const;
|
||||
|
||||
/** get functions for x, y */
|
||||
double x() const {return x_;}
|
||||
|
|
@ -40,7 +40,7 @@ namespace gtsam {
|
|||
/** return DOF, dimensionality of tangent space */
|
||||
size_t dim() const { return 2;}
|
||||
|
||||
/** Given 3-dim tangent vector, create new rotation */
|
||||
/** Given 2-dim tangent vector, create new point */
|
||||
Point2 exmap(const Vector& d) const {
|
||||
return Point2(x_+d(0),y_+d(1));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,88 +11,66 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Pose2::print(const string& s) const {
|
||||
cout << s << "(" << x_ << ", " << y_ << ", " << theta_ << ")" << endl;
|
||||
cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Pose2::equals(const Pose2& q, double tol) const {
|
||||
return (fabs(x_ - q.x_) < tol && fabs(y_ - q.y_) < tol && fabs(theta_
|
||||
- q.theta_) < tol);
|
||||
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::exmap(const Vector& v) const {
|
||||
return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
|
||||
return Pose2(t_+Point2(v(0),v(1)), r_.exmap(Vector_(1, v(2))));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose2::vector() const {
|
||||
Vector v(3);
|
||||
v(0) = x_;
|
||||
v(1) = y_;
|
||||
v(2) = theta_;
|
||||
return v;
|
||||
return Vector_(3, t_.x(), t_.y(), r_.theta());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::rotate(double theta) const {
|
||||
double c = cos(theta), s = sin(theta);
|
||||
return Pose2(c * x_ - s * y_, s * x_ + c * y_, theta + theta_);
|
||||
}
|
||||
// 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) {
|
||||
double dx = point.x()-pose.x(), dy = point.y()-pose.y();
|
||||
double ct=cos(pose.theta()), st=sin(pose.theta());
|
||||
return Point2(ct*dx + st*dy, -st*dx + ct*dy);
|
||||
return pose*point;
|
||||
}
|
||||
|
||||
// TODO, have a combined function that returns both function value and derivative
|
||||
Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
|
||||
double dx = point.x()-pose.x(), dy = point.y()-pose.y();
|
||||
double ct=cos(pose.theta()), st=sin(pose.theta());
|
||||
double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
|
||||
return Matrix_(2,3,
|
||||
-ct, -st, transformed_y,
|
||||
st, -ct, -transformed_x
|
||||
);
|
||||
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) {
|
||||
double ct=cos(pose.theta()), st=sin(pose.theta());
|
||||
return Matrix_(2,2,
|
||||
ct, st,
|
||||
-st, ct
|
||||
);
|
||||
return pose.r().transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 between(const Pose2& p1, const Pose2& p2) {
|
||||
double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
|
||||
double ct=cos(p1.theta()), st=sin(p1.theta());
|
||||
return Pose2(ct*dx + st*dy, -st*dx + ct*dy, p2.theta()-p1.theta());
|
||||
return Pose2(
|
||||
p1.r().invcompose(p2.r()),
|
||||
p1.r().unrotate(p2.t() - p1.t()));
|
||||
}
|
||||
|
||||
Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
|
||||
double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
|
||||
double ct=cos(p1.theta()), st=sin(p1.theta());
|
||||
double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
|
||||
return Matrix_(3,3,
|
||||
-ct, -st, transformed_y,
|
||||
st, -ct, -transformed_x,
|
||||
0.0, 0.0, -1.0
|
||||
);
|
||||
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) {
|
||||
double ct=cos(p1.theta()), st=sin(p1.theta());
|
||||
return Matrix_(3,3,
|
||||
ct, st, 0.0,
|
||||
-st, ct, 0.0,
|
||||
0.0, 0.0, 1.0
|
||||
);
|
||||
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
|
||||
|
|
|
|||
159
cpp/Pose2.h
159
cpp/Pose2.h
|
|
@ -1,7 +1,8 @@
|
|||
/**
|
||||
* @file Pose2.h
|
||||
* @brief 3D Pose
|
||||
* @brief 2D Pose
|
||||
* @author: Frank Dellaert
|
||||
* @author: Richard Roberts
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
|
@ -9,93 +10,117 @@
|
|||
#pragma once
|
||||
|
||||
#include "Point2.h"
|
||||
#include "Rot2.h"
|
||||
#include "Matrix.h"
|
||||
#include "Testable.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A 2D pose (x,y,theta)
|
||||
*/
|
||||
class Pose2: Testable<Pose2> {
|
||||
/**
|
||||
* A 2D pose (Point2,Rot2)
|
||||
*/
|
||||
class Pose2: Testable<Pose2> {
|
||||
|
||||
private:
|
||||
double x_, y_, theta_;
|
||||
private:
|
||||
Point2 t_;
|
||||
Rot2 r_;
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/** default constructor = origin */
|
||||
Pose2() :
|
||||
x_(0), y_(0), theta_(0) {
|
||||
} // default is origin
|
||||
/** default constructor = origin */
|
||||
Pose2() :
|
||||
t_(0.0, 0.0), r_(0) { } // default is origin
|
||||
|
||||
/** copy constructor */
|
||||
Pose2(const Pose2& pose) :
|
||||
x_(pose.x_), y_(pose.y_), theta_(pose.theta_) {
|
||||
}
|
||||
/** copy constructor */
|
||||
Pose2(const Pose2& pose) :
|
||||
t_(pose.t_), r_(pose.r_) { }
|
||||
|
||||
/**
|
||||
* construct from (x,y,theta)
|
||||
* @param x x oordinate
|
||||
* @param y y coordinate
|
||||
* @param theta angle with positive X-axis
|
||||
*/
|
||||
Pose2(double x, double y, double theta) :
|
||||
x_(x), y_(y), theta_(theta) {
|
||||
}
|
||||
/**
|
||||
* 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) :
|
||||
x_(t.x()), y_(t.y()), theta_(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;
|
||||
/** 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;
|
||||
/** assert equality up to a tolerance */
|
||||
bool equals(const Pose2& pose, double tol = 1e-9) const;
|
||||
|
||||
/** get functions for x, y, theta */
|
||||
inline double x() const { return x_;}
|
||||
inline double y() const { return y_;}
|
||||
inline double theta() const { return theta_;}
|
||||
/** 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;
|
||||
}
|
||||
/** return DOF, dimensionality of tangent space = 3 */
|
||||
size_t dim() const { return 3; }
|
||||
|
||||
/* exponential map */
|
||||
Pose2 exmap(const Vector& v) const;
|
||||
/* exponential map */
|
||||
Pose2 exmap(const Vector& v) const;
|
||||
|
||||
/** return vectorized form (column-wise)*/
|
||||
Vector vector() const;
|
||||
/** return vectorized form (column-wise) */
|
||||
Vector vector() const;
|
||||
|
||||
/** rotate pose by theta */
|
||||
Pose2 rotate(double theta) const;
|
||||
/** rotate pose by theta */
|
||||
// Pose2 rotate(double theta) const;
|
||||
|
||||
// operators
|
||||
Pose2 operator+(const Pose2& p2) const {
|
||||
return Pose2(x_ + p2.x_, y_ + p2.y_, theta_ + p2.theta_);
|
||||
}
|
||||
/** inverse transformation */
|
||||
Pose2 inverse() const {
|
||||
return Pose2(r_, r_.unrotate(t_));
|
||||
}
|
||||
|
||||
Pose2 operator-(const Pose2& p2) const {
|
||||
return Pose2(x_ - p2.x_, y_ - p2.y_, theta_ - p2.theta_);
|
||||
}
|
||||
}; // Pose2
|
||||
/** 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_);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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);
|
||||
/** same as compose (pre-multiply this*p1) */
|
||||
Pose2 operator*(const Pose2& p1) const {
|
||||
return compose(p1);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 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);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
18
cpp/Rot2.cpp
18
cpp/Rot2.cpp
|
|
@ -29,19 +29,29 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot2::matrix() const {
|
||||
double r[] = { c_, -s_, s_, c_ };
|
||||
return Matrix_(2, 2, r);
|
||||
return Matrix_(2, 2, c_, -s_, s_, c_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot2::transpose() const {
|
||||
double r[] = { c_, s_, -s_, c_ };
|
||||
return Matrix_(2, 2, r);
|
||||
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::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(
|
||||
|
|
|
|||
17
cpp/Rot2.h
17
cpp/Rot2.h
|
|
@ -34,7 +34,14 @@ namespace gtsam {
|
|||
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
|
||||
|
||||
/** return angle */
|
||||
inline double angle() const { return atan2(s_, c_); }
|
||||
double theta() const { return atan2(s_,c_); }
|
||||
double angle() const { return atan2(s_,c_); }
|
||||
|
||||
/** return cos */
|
||||
double c() const { return c_; }
|
||||
|
||||
/** return sin */
|
||||
double s() const { return s_; }
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "theta") const;
|
||||
|
|
@ -54,12 +61,18 @@ namespace gtsam {
|
|||
/** return 2*2 rotation matrix */
|
||||
Matrix matrix() const;
|
||||
|
||||
/** return 3*3 transpose (inverse) rotation matrix */
|
||||
/** return 2*2 transpose (inverse) rotation matrix */
|
||||
Matrix transpose() const;
|
||||
|
||||
/** return 2*2 negative transpose */
|
||||
Matrix negtranspose() const;
|
||||
|
||||
/** inverse transformation */
|
||||
Rot2 inverse() 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;
|
||||
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
#include "numericalDerivative.h"
|
||||
#include "Pose2.h"
|
||||
#include "Point2.h"
|
||||
#include "Rot2.h"
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
|
|
@ -20,13 +21,22 @@ TEST(Pose2, constructors) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, rotate) {
|
||||
double theta = 0.1, c=cos(theta),s=sin(theta);
|
||||
Pose2 p1(1,0,0.2), p2(0,1,0.4);
|
||||
CHECK(assert_equal(Pose2( c,s,0.3),p1.rotate(theta)));
|
||||
CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
|
||||
TEST(Pose2, exmap) {
|
||||
Pose2 pose(Point2(1,2), M_PI_2);
|
||||
Pose2 expected(Point2(1.01, 1.985), M_PI_2+0.018);
|
||||
Pose2 actual = pose.exmap(Vector_(3, 0.01, -0.015, 0.018));
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TEST(Pose2, rotate) {
|
||||
// std::cout << "rotate\n";
|
||||
// double theta = 0.1, c=cos(theta),s=sin(theta);
|
||||
// Pose2 p1(1,0,0.2), p2(0,1,0.4);
|
||||
// CHECK(assert_equal(Pose2( c,s,0.3),p1.rotate(theta)));
|
||||
// CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, operators) {
|
||||
CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1)));
|
||||
|
|
@ -60,6 +70,37 @@ TEST( Pose2, transform_to )
|
|||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, compose_a)
|
||||
{
|
||||
Pose2 pose1(Point2(.75, .5), Rot2(M_PI/10.0));
|
||||
Pose2 pose2(Point2(0.701289620636, 1.34933052585), Rot2(M_PI/4.0-M_PI/10.0));
|
||||
|
||||
Pose2 pose_expected(Point2(1.0, 2.0), Rot2(M_PI/4.0));
|
||||
|
||||
Pose2 pose_actual_op = pose2 * pose1;
|
||||
Pose2 pose_actual_fcn = pose2.compose(pose1);
|
||||
|
||||
CHECK(assert_equal(pose_expected, pose_actual_op));
|
||||
CHECK(assert_equal(pose_expected, pose_actual_fcn));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, compose_b)
|
||||
{
|
||||
Pose2 pose1(Point2(1.0, 1.0), Rot2(M_PI/4.0));
|
||||
Pose2 pose2(Point2(sqrt(.5), sqrt(.5)), Rot2(M_PI/4.0));
|
||||
|
||||
Pose2 pose_expected(Point2(1.0, 2.0), Rot2(M_PI/2.0));
|
||||
|
||||
Pose2 pose_actual_op = pose2 * pose1;
|
||||
Pose2 pose_actual_fcn = pose2.compose(pose1);
|
||||
|
||||
CHECK(assert_equal(pose_expected, pose_actual_op));
|
||||
CHECK(assert_equal(pose_expected, pose_actual_fcn));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose2, between )
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue