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

Moved transform_[to|from] and [un]rotate to be static functions in classes

release/4.3a0
Alex Cunningham 2010-08-20 21:47:30 +00:00
parent 8c33168fb3
commit 256697d345
20 changed files with 179 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
geometry/CalibratedCamera.cpp
View File

@ -49,7 +49,7 @@ namespace gtsam {
} }
Point2 CalibratedCamera::project(const Point3 & P) const { Point2 CalibratedCamera::project(const Point3 & P) const {
Point3 cameraPoint = transform_to(pose_, P); Point3 cameraPoint = Pose3::transform_to(pose_, P);
Point2 intrinsic = project_to_camera(cameraPoint); Point2 intrinsic = project_to_camera(cameraPoint);
return intrinsic; return intrinsic;
} }
@ -67,7 +67,7 @@ namespace gtsam {
const Pose3& pose = camera.pose(); const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation(); const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = transform_to(pose, point); Point3 q = Pose3::transform_to(pose, point);
double X = q.x(), Y = q.y(), Z = q.z(); double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2; double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
return Matrix_(2,6, return Matrix_(2,6,
@ -80,7 +80,7 @@ namespace gtsam {
const Pose3& pose = camera.pose(); const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation(); const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = transform_to(pose, point); Point3 q = Pose3::transform_to(pose, point);
double X = q.x(), Y = q.y(), Z = q.z(); double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2; double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
return Matrix_(2,3, return Matrix_(2,3,
@ -95,7 +95,7 @@ namespace gtsam {
const Pose3& pose = camera.pose(); const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation(); const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = transform_to(pose, point); Point3 q = Pose3::transform_to(pose, point);
double X = q.x(), Y = q.y(), Z = q.z(); double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2; double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2; double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2;

14
geometry/Point3.cpp
View File

@ -48,41 +48,41 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 add(const Point3 &p, const Point3 &q) { Point3 Point3::add(const Point3 &p, const Point3 &q) {
return p+q; return p+q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 add(const Point3 &p, const Point3 &q, Point3 Point3::add(const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = eye(3,3); if (H1) *H1 = eye(3,3);
if (H2) *H2 = eye(3,3); if (H2) *H2 = eye(3,3);
return add(p,q); return add(p,q);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 sub(const Point3 &p, const Point3 &q) { Point3 Point3::sub(const Point3 &p, const Point3 &q) {
return p+q; return p+q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 sub(const Point3 &p, const Point3 &q, Point3 Point3::sub(const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = eye(3,3); if (H1) *H1 = eye(3,3);
if (H2) *H2 = -eye(3,3); if (H2) *H2 = -eye(3,3);
return sub(p,q); return sub(p,q);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 cross(const Point3 &p, const Point3 &q) Point3 Point3::cross(const Point3 &p, const Point3 &q)
{ {
return Point3( p.y_*q.z_ - p.z_*q.y_, return Point3( p.y_*q.z_ - p.z_*q.y_,
p.z_*q.x_ - p.x_*q.z_, p.z_*q.x_ - p.x_*q.z_,
p.x_*q.y_ - p.y_*q.x_ ); p.x_*q.y_ - p.y_*q.x_ );
} }
/* ************************************************************************* */ /* ************************************************************************* */
double dot(const Point3 &p, const Point3 &q) double Point3::dot(const Point3 &p, const Point3 &q)
{ {
return ( p.x_*q.x_ + p.y_*q.y_ + p.z_*q.z_ ); return ( p.x_*q.x_ + p.y_*q.y_ + p.z_*q.z_ );
} }
/* ************************************************************************* */ /* ************************************************************************* */
double norm(const Point3 &p) double Point3::norm(const Point3 &p)
{ {
return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ ); return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ );
} }

45
geometry/Point3.h
View File

@ -84,10 +84,29 @@ namespace gtsam {
return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
} }
/** friends */ /** add two points, add(p,q) is same as p+q */
friend Point3 cross(const Point3 &p1, const Point3 &p2); static Point3 add (const Point3 &p, const Point3 &q);
friend double dot(const Point3 &p1, const Point3 &p2); static Point3 add (const Point3 &p, const Point3 &q,
friend double norm(const Point3 &p1); boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
/** subtract two points, sub(p,q) is same as p-q */
static Point3 sub (const Point3 &p, const Point3 &q);
static Point3 sub (const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
/** cross product */
static Point3 cross(const Point3 &p, const Point3 &q);
/** dot product */
static double dot(const Point3 &p, const Point3 &q);
/** dot product */
static double norm(const Point3 &p);
// /** friends */
// friend Point3 cross(const Point3 &p1, const Point3 &p2);
// friend double dot(const Point3 &p1, const Point3 &p2);
// friend double norm(const Point3 &p1);
private: private:
/** Serialization function */ /** Serialization function */
@ -111,22 +130,4 @@ namespace gtsam {
/** Syntactic sugar for multiplying coordinates by a scalar s*p */ /** Syntactic sugar for multiplying coordinates by a scalar s*p */
inline Point3 operator*(double s, const Point3& p) { return p*s;} inline Point3 operator*(double s, const Point3& p) { return p*s;}
/** add two points, add(p,q) is same as p+q */
Point3 add (const Point3 &p, const Point3 &q);
Point3 add (const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
/** subtract two points, sub(p,q) is same as p-q */
Point3 sub (const Point3 &p, const Point3 &q);
Point3 sub (const Point3 &p, const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
/** cross product */
Point3 cross(const Point3 &p, const Point3 &q);
/** dot product */
double dot(const Point3 &p, const Point3 &q);
/** dot product */
double norm(const Point3 &p);
} }

12
geometry/Pose2.cpp
View File

@ -108,7 +108,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SE(2) section // see doc/math.lyx, SE(2) section
Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional< Point2 Pose2::transform_to(const Pose2& pose, const Point2& point, boost::optional<
Matrix&> H1, boost::optional<Matrix&> H2) { Matrix&> H1, boost::optional<Matrix&> H2) {
const Rot2& R = pose.r(); const Rot2& R = pose.r();
Point2 d = point - pose.t(); Point2 d = point - pose.t();
@ -134,7 +134,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SE(2) section // see doc/math.lyx, SE(2) section
Point2 transform_from(const Pose2& pose, const Point2& p, Point2 Pose2::transform_from(const Pose2& pose, const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
const Rot2& rot = pose.r(); const Rot2& rot = pose.r();
const Point2 q = rot * p; const Point2 q = rot * p;
@ -181,14 +181,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Rot2 bearing(const Pose2& pose, const Point2& point) { Rot2 bearing(const Pose2& pose, const Point2& point) {
Point2 d = transform_to(pose, point); Point2 d = Pose2::transform_to(pose, point);
return Rot2::relativeBearing(d); return Rot2::relativeBearing(d);
} }
Rot2 bearing(const Pose2& pose, const Point2& point, Rot2 bearing(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return bearing(pose, point); if (!H1 && !H2) return bearing(pose, point);
Point2 d = transform_to(pose, point, H1, H2); Point2 d = Pose2::transform_to(pose, point, H1, H2);
Matrix D_result_d; Matrix D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d); Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * (*H1); if (H1) *H1 = D_result_d * (*H1);
@ -198,14 +198,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
double range(const Pose2& pose, const Point2& point) { double range(const Pose2& pose, const Point2& point) {
Point2 d = transform_to(pose, point); Point2 d = Pose2::transform_to(pose, point);
return d.norm(); return d.norm();
} }
double range(const Pose2& pose, const Point2& point, double range(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return range(pose, point); if (!H1 && !H2) return range(pose, point);
Point2 d = transform_to(pose, point, H1, H2); Point2 d = Pose2::transform_to(pose, point, H1, H2);
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
Matrix D_result_d = Matrix_(1, 2, x / n, y / n); Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
if (H1) *H1 = D_result_d * (*H1); if (H1) *H1 = D_result_d * (*H1);

34
geometry/Pose2.h
View File

@ -94,6 +94,22 @@ namespace gtsam {
/** return transformation matrix */ /** return transformation matrix */
Matrix matrix() const; Matrix matrix() const;
/**
* Return point coordinates in pose coordinate frame
*/
static inline Point2 transform_to(const Pose2& pose, const Point2& point)
{ return Rot2::unrotate(pose.r(), point - pose.t());}
static Point2 transform_to(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* Return point coordinates in global frame
*/
static inline Point2 transform_from(const Pose2& pose, const Point2& point)
{ return Rot2::rotate(pose.r(), point) + pose.t();}
static Point2 transform_from(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** get functions for x, y, theta */ /** get functions for x, y, theta */
inline double x() const { return t_.x(); } inline double x() const { return t_.x(); }
inline double y() const { return t_.y(); } inline double y() const { return t_.y(); }
@ -150,23 +166,9 @@ namespace gtsam {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2 = boost::none); boost::optional<Matrix&> H2 = boost::none);
/** /** syntactic sugar for transform_from */
* Return point coordinates in pose coordinate frame
*/
inline Point2 transform_to(const Pose2& pose, const Point2& point)
{ return unrotate(pose.r(), point - pose.t());}
Point2 transform_to(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* Return point coordinates in global frame
*/
inline Point2 transform_from(const Pose2& pose, const Point2& point)
{ return rotate(pose.r(), point) + pose.t();}
Point2 transform_from(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
inline Point2 operator*(const Pose2& pose, const Point2& point) inline Point2 operator*(const Pose2& pose, const Point2& point)
{ return transform_from(pose, point);} { return Pose2::transform_from(pose, point);}
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame

22
geometry/Pose3.cpp
View File

@ -134,17 +134,17 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const { Pose3 Pose3::transform_to(const Pose3& pose) const {
Rot3 cRv = R_ * Rot3(gtsam::inverse(pose.R_)); Rot3 cRv = R_ * Rot3(gtsam::inverse(pose.R_));
Point3 t = gtsam::transform_to(pose, t_); Point3 t = Pose3::transform_to(pose, t_);
return Pose3(cRv, t); return Pose3(cRv, t);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 transform_from(const Pose3& pose, const Point3& p) { Point3 Pose3::transform_from(const Pose3& pose, const Point3& p) {
return pose.rotation() * p + pose.translation(); return pose.rotation() * p + pose.translation();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 transform_from(const Pose3& pose, const Point3& p, Point3 Pose3::transform_from(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) { if (H1) {
#ifdef CORRECT_POSE3_EXMAP #ifdef CORRECT_POSE3_EXMAP
@ -153,22 +153,22 @@ namespace gtsam {
*H1 = collect(2,&DR,&R); *H1 = collect(2,&DR,&R);
#else #else
Matrix DR; Matrix DR;
rotate(pose.rotation(), p, DR, boost::none); Rot3::rotate(pose.rotation(), p, DR, boost::none);
*H1 = collect(2,&DR,&I3); *H1 = collect(2,&DR,&I3);
#endif #endif
} }
if (H2) *H2 = pose.rotation().matrix(); if (H2) *H2 = pose.rotation().matrix();
return transform_from(pose, p); return Pose3::transform_from(pose, p);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 transform_to(const Pose3& pose, const Point3& p) { Point3 Pose3::transform_to(const Pose3& pose, const Point3& p) {
Point3 sub = p - pose.translation(); Point3 sub = p - pose.translation();
return unrotate(pose.rotation(), sub); return Rot3::unrotate(pose.rotation(), sub);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 transform_to(const Pose3& pose, const Point3& p, Point3 Pose3::transform_to(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) { // *H1 = Dtransform_to1(pose, p); if (H1) { // *H1 = Dtransform_to1(pose, p);
Point3 q = transform_to(pose,p); Point3 q = transform_to(pose,p);
@ -182,7 +182,7 @@ namespace gtsam {
} }
if (H2) *H2 = pose.rotation().transpose(); if (H2) *H2 = pose.rotation().transpose();
return transform_to(pose, p); return Pose3::transform_to(pose, p);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -197,7 +197,7 @@ namespace gtsam {
Matrix DR_R1 = R2.transpose(), DR_t1 = Z3; Matrix DR_R1 = R2.transpose(), DR_t1 = Z3;
Matrix DR = collect(2, &DR_R1, &DR_t1); Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt; Matrix Dt;
transform_from(p1,t2, Dt, boost::none); Pose3::transform_from(p1,t2, Dt, boost::none);
*H1 = gtsam::stack(2, &DR, &Dt); *H1 = gtsam::stack(2, &DR, &Dt);
#endif #endif
} }
@ -226,7 +226,7 @@ namespace gtsam {
const Point3& t = p.translation(); const Point3& t = p.translation();
Matrix Rt = R.transpose(); Matrix Rt = R.transpose();
Matrix DR_R1 = -R.matrix(), DR_t1 = Z3; Matrix DR_R1 = -R.matrix(), DR_t1 = Z3;
Matrix Dt_R1 = -skewSymmetric(unrotate(R,t).vector()), Dt_t1 = -Rt; Matrix Dt_R1 = -skewSymmetric(Rot3::unrotate(R,t).vector()), Dt_t1 = -Rt;
Matrix DR = collect(2, &DR_R1, &DR_t1); Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt = collect(2, &Dt_R1, &Dt_t1); Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
*H1 = gtsam::stack(2, &DR, &Dt); *H1 = gtsam::stack(2, &DR, &Dt);

22
geometry/Pose3.h
View File

@ -86,6 +86,16 @@ namespace gtsam {
/** composes two poses */ /** composes two poses */
inline Pose3 compose(const Pose3& t) const { return *this * t; } inline Pose3 compose(const Pose3& t) const { return *this * t; }
/** receives the point in Pose coordinates and transforms it to world coordinates */
static Point3 transform_from(const Pose3& pose, const Point3& p);
static Point3 transform_from(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** receives the point in world coordinates and transforms it to Pose coordinates */
static Point3 transform_to(const Pose3& pose, const Point3& p);
static Point3 transform_to(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** Exponential map at identity - create a pose with a translation and /** Exponential map at identity - create a pose with a translation and
* rotation (in canonical coordinates). */ * rotation (in canonical coordinates). */
static Pose3 Expmap(const Vector& v); static Pose3 Expmap(const Vector& v);
@ -110,16 +120,8 @@ namespace gtsam {
/** Logarithm map around another pose T1 */ /** Logarithm map around another pose T1 */
Vector logmap(const Pose3& T1, const Pose3& T2); Vector logmap(const Pose3& T1, const Pose3& T2);
/** receives the point in Pose coordinates and transforms it to world coordinates */ /** syntactic sugar for transform */
Point3 transform_from(const Pose3& pose, const Point3& p); inline Point3 operator*(const Pose3& pose, const Point3& p) { return Pose3::transform_from(pose, p); }
inline Point3 operator*(const Pose3& pose, const Point3& p) { return transform_from(pose, p); }
Point3 transform_from(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Pose3& pose, const Point3& p);
Point3 transform_to(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** /**
* Derivatives of compose * Derivatives of compose

4
geometry/Rot2.cpp
View File

@ -74,7 +74,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SO(2) section // see doc/math.lyx, SO(2) section
Point2 rotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1, Point2 Rot2::rotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) { boost::optional<Matrix&> H2) {
Point2 q = R * p; Point2 q = R * p;
if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x()); if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x());
@ -84,7 +84,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SO(2) section // see doc/math.lyx, SO(2) section
Point2 unrotate(const Rot2 & R, const Point2& p, Point2 Rot2::unrotate(const Rot2 & R, const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Point2 q = R.unrotate(p); Point2 q = R.unrotate(p);
if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x()); // R_{pi/2}q if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x()); // R_{pi/2}q

39
geometry/Rot2.h
View File

@ -44,7 +44,9 @@ namespace gtsam {
/** "named constructors" */ /** "named constructors" */
/** Named constructor from angle == exponential map at identity - theta is in radians*/ /** Named constructor from angle == exponential map at identity - theta is in radians*/
static Rot2 fromAngle(double theta); static Rot2 fromAngle(double theta) {
return Rot2(cos(theta), sin(theta));
}
/** Named constructor from angle in degrees */ /** Named constructor from angle in degrees */
static Rot2 fromDegrees(double theta) { static Rot2 fromDegrees(double theta) {
@ -134,9 +136,23 @@ namespace gtsam {
/** rotate from world to rotated = R*p */ /** rotate from world to rotated = R*p */
Point2 rotate(const Point2& p) const; Point2 rotate(const Point2& p) const;
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
static Point2 rotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
/** rotate from world to rotated = R'*p */ /** rotate from world to rotated = R'*p */
Point2 unrotate(const Point2& p) const; Point2 unrotate(const Point2& p) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
static Point2 unrotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -148,27 +164,8 @@ namespace gtsam {
}; // Rot2 }; // Rot2
/* inline named constructor implementation */ /** syntactic sugar for rotate */
inline Rot2 Rot2::fromAngle(double theta) {
return Rot2(cos(theta), sin(theta));
}
// Lie group functions
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
inline Point2 operator*(const Rot2& R, const Point2& p) {return R.rotate(p);} inline Point2 operator*(const Rot2& R, const Point2& p) {return R.rotate(p);}
Point2 rotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point2 unrotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
} // gtsam } // gtsam

6
geometry/Rot3.cpp
View File

@ -166,7 +166,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 rotate(const Rot3& R, const Point3& p, Point3 Rot3::rotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H1) *H1 = R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R.matrix(); if (H2) *H2 = R.matrix();
@ -174,14 +174,14 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 unrotate(const Rot3& R, const Point3& p) { Point3 Rot3::unrotate(const Rot3& R, const Point3& p) {
const Matrix Rt(R.transpose()); const Matrix Rt(R.transpose());
return Rt*p.vector(); // q = Rt*p return Rt*p.vector(); // q = Rt*p
} }
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SO(3) section // see doc/math.lyx, SO(3) section
Point3 unrotate(const Rot3& R, const Point3& p, Point3 Rot3::unrotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
const Matrix Rt(R.transpose()); const Matrix Rt(R.transpose());
Point3 q(Rt*p.vector()); // q = Rt*p Point3 q(Rt*p.vector()); // q = Rt*p

32
geometry/Rot3.h
View File

@ -177,6 +177,22 @@ namespace gtsam {
{return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();} {return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();}
inline Point3 operator*(const Point3& p) const { return rotate(p);} inline Point3 operator*(const Point3& p) const { return rotate(p);}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
static inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;}
static Point3 rotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* rotate point from world to rotated
* frame = R'*p
*/
static Point3 unrotate(const Rot3& R, const Point3& p);
static Point3 unrotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -195,22 +211,6 @@ namespace gtsam {
return R.inverse(); return R.inverse();
} }
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;}
Point3 rotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Rot3& R, const Point3& p);
Point3 unrotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** /**
* compose two rotations i.e., R=R1*R2 * compose two rotations i.e., R=R1*R2
*/ */

4
geometry/SimpleCamera.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
} }
pair<Point2, bool> SimpleCamera::projectSafe(const Point3& P) const { pair<Point2, bool> SimpleCamera::projectSafe(const Point3& P) const {
Point3 cameraPoint = transform_to(calibrated_.pose(), P); Point3 cameraPoint = Pose3::transform_to(calibrated_.pose(), P);
Point2 intrinsic = project_to_camera(cameraPoint); Point2 intrinsic = project_to_camera(cameraPoint);
Point2 projection = uncalibrate(K_, intrinsic); Point2 projection = uncalibrate(K_, intrinsic);
return pair<Point2, bool>(projection, cameraPoint.z() > 0); return pair<Point2, bool>(projection, cameraPoint.z() > 0);
@ -41,7 +41,7 @@ namespace gtsam {
Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const { Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_.calibrate(projection); Point2 intrinsic = K_.calibrate(projection);
Point3 cameraPoint = backproject_from_camera(intrinsic, scale); Point3 cameraPoint = backproject_from_camera(intrinsic, scale);
return transform_from(calibrated_.pose(), cameraPoint); return Pose3::transform_from(calibrated_.pose(), cameraPoint);
} }
SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) { SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) {

6
geometry/StereoCamera.cpp
View File

@ -24,7 +24,7 @@ StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double ba
/* ************************************************************************* */ /* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point) const { StereoPoint2 StereoCamera::project(const Point3& point) const {
Point3 cameraPoint = transform_to(leftCamPose_, point); Point3 cameraPoint = Pose3::transform_to(leftCamPose_, point);
double d = 1.0 / cameraPoint.z(); double d = 1.0 / cameraPoint.z();
double uL = cx_ + d * fx_ * cameraPoint.x(); double uL = cx_ + d * fx_ * cameraPoint.x();
@ -58,7 +58,7 @@ Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point) {
//Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point); //Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
//**** above function call inlined //**** above function call inlined
Matrix D_cameraPoint_pose; Matrix D_cameraPoint_pose;
Point3 cameraPoint = transform_to(camera.pose(), point, D_cameraPoint_pose, boost::none); Point3 cameraPoint = Pose3::transform_to(camera.pose(), point, D_cameraPoint_pose, boost::none);
//cout << "D_cameraPoint_pose" << endl; //cout << "D_cameraPoint_pose" << endl;
//print(D_cameraPoint_pose); //print(D_cameraPoint_pose);
@ -84,7 +84,7 @@ Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point) {
//Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point); //Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
//**** above function call inlined //**** above function call inlined
Matrix D_cameraPoint_point; Matrix D_cameraPoint_point;
Point3 cameraPoint = transform_to(camera.pose(), point, boost::none, D_cameraPoint_point); Point3 cameraPoint = Pose3::transform_to(camera.pose(), point, boost::none, D_cameraPoint_point);
//Point2 intrinsic = project_to_camera(cameraPoint); // unused //Point2 intrinsic = project_to_camera(cameraPoint); // unused
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian

4
geometry/tests/testPoint3.cpp
View File

@ -33,8 +33,8 @@ TEST( Point3, equals)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point3, dot) TEST( Point3, dot)
{ {
CHECK(dot(Point3(0,0,0),Point3(1,1,0)) == 0); CHECK(Point3::dot(Point3(0,0,0),Point3(1,1,0)) == 0);
CHECK(dot(Point3(1,1,1),Point3(1,1,0)) == 2); CHECK(Point3::dot(Point3(1,1,1),Point3(1,1,0)) == 2);
} }
/* ************************************************************************* */ /* ************************************************************************* */

20
geometry/tests/testPose2.cpp
View File

@ -129,29 +129,25 @@ TEST( Pose2, transform_to )
// actual // actual
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
Point2 actual = transform_to(pose,point, actualH1, actualH2); Point2 actual = Pose2::transform_to(pose,point, actualH1, actualH2);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
CHECK(assert_equal(expectedH1,actualH1)); CHECK(assert_equal(expectedH1,actualH1));
Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5); Matrix numericalH1 = numericalDerivative21(Pose2::transform_to, pose, point, 1e-5);
CHECK(assert_equal(numericalH1,actualH1)); CHECK(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2)); CHECK(assert_equal(expectedH2,actualH2));
Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5); Matrix numericalH2 = numericalDerivative22(Pose2::transform_to, pose, point, 1e-5);
CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
return transform_from(pose, point);
}
TEST (Pose2, transform_from) TEST (Pose2, transform_from)
{ {
Pose2 pose(1., 0., M_PI_2); Pose2 pose(1., 0., M_PI_2);
Point2 pt(2., 1.); Point2 pt(2., 1.);
Matrix H1, H2; Matrix H1, H2;
Point2 actual = transform_from(pose, pt, H1, H2); Point2 actual = Pose2::transform_from(pose, pt, H1, H2);
Point2 expected(0., 2.); Point2 expected(0., 2.);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
@ -159,11 +155,11 @@ TEST (Pose2, transform_from)
Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.); Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.); Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.);
Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt, 1e-5); Matrix numericalH1 = numericalDerivative21(Pose2::transform_from, pose, pt, 1e-5);
CHECK(assert_equal(H1_expected, H1)); CHECK(assert_equal(H1_expected, H1));
CHECK(assert_equal(H1_expected, numericalH1)); CHECK(assert_equal(H1_expected, numericalH1));
Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt, 1e-5); Matrix numericalH2 = numericalDerivative22(Pose2::transform_from, pose, pt, 1e-5);
CHECK(assert_equal(H2_expected, H2)); CHECK(assert_equal(H2_expected, H2));
CHECK(assert_equal(H2_expected, numericalH2)); CHECK(assert_equal(H2_expected, numericalH2));
} }
@ -197,8 +193,8 @@ TEST(Pose2, compose_a)
Point2 point(sqrt(0.5), 3.0*sqrt(0.5)); Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
Point2 expected_point(-1.0, -1.0); Point2 expected_point(-1.0, -1.0);
Point2 actual_point1 = transform_to(pose1 * pose2, point); Point2 actual_point1 = Pose2::transform_to(pose1 * pose2, point);
Point2 actual_point2 = transform_to(pose2, transform_to(pose1, point)); Point2 actual_point2 = Pose2::transform_to(pose2, Pose2::transform_to(pose1, point));
CHECK(assert_equal(expected_point, actual_point1)); CHECK(assert_equal(expected_point, actual_point1));
CHECK(assert_equal(expected_point, actual_point2)); CHECK(assert_equal(expected_point, actual_point2));
} }

50
geometry/tests/testPose3.cpp
View File

@ -219,8 +219,8 @@ TEST( Pose3, compose_inverse)
TEST( Pose3, Dtransform_from1_a) TEST( Pose3, Dtransform_from1_a)
{ {
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
transform_from(T, P, actualDtransform_from1, boost::none); Pose3::transform_from(T, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from,T,P); Matrix numerical = numericalDerivative21(Pose3::transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
@ -228,8 +228,8 @@ TEST( Pose3, Dtransform_from1_b)
{ {
Pose3 origin; Pose3 origin;
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
transform_from(origin, P, actualDtransform_from1, boost::none); Pose3::transform_from(origin, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from,origin,P); Matrix numerical = numericalDerivative21(Pose3::transform_from,origin,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
@ -238,8 +238,8 @@ TEST( Pose3, Dtransform_from1_c)
Point3 origin; Point3 origin;
Pose3 T0(R,origin); Pose3 T0(R,origin);
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
transform_from(T0, P, actualDtransform_from1, boost::none); Pose3::transform_from(T0, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from,T0,P); Matrix numerical = numericalDerivative21(Pose3::transform_from,T0,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
@ -249,9 +249,9 @@ TEST( Pose3, Dtransform_from1_d)
Point3 t0(100,0,0); Point3 t0(100,0,0);
Pose3 T0(I,t0); Pose3 T0(I,t0);
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
transform_from(T0, P, actualDtransform_from1, boost::none); Pose3::transform_from(T0, P, actualDtransform_from1, boost::none);
//print(computed, "Dtransform_from1_d computed:"); //print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(transform_from,T0,P); Matrix numerical = numericalDerivative21(Pose3::transform_from,T0,P);
//print(numerical, "Dtransform_from1_d numerical:"); //print(numerical, "Dtransform_from1_d numerical:");
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
@ -260,8 +260,8 @@ TEST( Pose3, Dtransform_from1_d)
TEST( Pose3, Dtransform_from2) TEST( Pose3, Dtransform_from2)
{ {
Matrix actualDtransform_from2; Matrix actualDtransform_from2;
transform_from(T,P, boost::none, actualDtransform_from2); Pose3::transform_from(T,P, boost::none, actualDtransform_from2);
Matrix numerical = numericalDerivative22(transform_from,T,P); Matrix numerical = numericalDerivative22(Pose3::transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from2,1e-8)); CHECK(assert_equal(numerical,actualDtransform_from2,1e-8));
} }
@ -269,8 +269,8 @@ TEST( Pose3, Dtransform_from2)
TEST( Pose3, Dtransform_to1) TEST( Pose3, Dtransform_to1)
{ {
Matrix computed; Matrix computed;
transform_to(T, P, computed, boost::none); Pose3::transform_to(T, P, computed, boost::none);
Matrix numerical = numericalDerivative21(transform_to,T,P); Matrix numerical = numericalDerivative21(Pose3::transform_to,T,P);
CHECK(assert_equal(numerical,computed,1e-8)); CHECK(assert_equal(numerical,computed,1e-8));
} }
@ -278,8 +278,8 @@ TEST( Pose3, Dtransform_to1)
TEST( Pose3, Dtransform_to2) TEST( Pose3, Dtransform_to2)
{ {
Matrix computed; Matrix computed;
transform_to(T,P, boost::none, computed); Pose3::transform_to(T,P, boost::none, computed);
Matrix numerical = numericalDerivative22(transform_to,T,P); Matrix numerical = numericalDerivative22(Pose3::transform_to,T,P);
CHECK(assert_equal(numerical,computed,1e-8)); CHECK(assert_equal(numerical,computed,1e-8));
} }
@ -287,9 +287,9 @@ TEST( Pose3, Dtransform_to2)
TEST( Pose3, transform_to_with_derivatives) TEST( Pose3, transform_to_with_derivatives)
{ {
Matrix actH1, actH2; Matrix actH1, actH2;
transform_to(T,P,actH1,actH2); Pose3::transform_to(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_to, T,P), Matrix expH1 = numericalDerivative21(Pose3::transform_to, T,P),
expH2 = numericalDerivative22(transform_to, T,P); expH2 = numericalDerivative22(Pose3::transform_to, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8)); CHECK(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8)); CHECK(assert_equal(expH2, actH2, 1e-8));
} }
@ -298,9 +298,9 @@ TEST( Pose3, transform_to_with_derivatives)
TEST( Pose3, transform_from_with_derivatives) TEST( Pose3, transform_from_with_derivatives)
{ {
Matrix actH1, actH2; Matrix actH1, actH2;
transform_from(T,P,actH1,actH2); Pose3::transform_from(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_from, T,P), Matrix expH1 = numericalDerivative21(Pose3::transform_from, T,P),
expH2 = numericalDerivative22(transform_from, T,P); expH2 = numericalDerivative22(Pose3::transform_from, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8)); CHECK(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8)); CHECK(assert_equal(expH2, actH2, 1e-8));
} }
@ -308,7 +308,7 @@ TEST( Pose3, transform_from_with_derivatives)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_to_translate) TEST( Pose3, transform_to_translate)
{ {
Point3 actual = transform_to(Pose3(Rot3(), Point3(1, 2, 3)), Point3(10.,20.,30.)); Point3 actual = Pose3::transform_to(Pose3(Rot3(), Point3(1, 2, 3)), Point3(10.,20.,30.));
Point3 expected(9.,18.,27.); Point3 expected(9.,18.,27.);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
@ -317,7 +317,7 @@ TEST( Pose3, transform_to_translate)
TEST( Pose3, transform_to_rotate) TEST( Pose3, transform_to_rotate)
{ {
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3()); Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3());
Point3 actual = transform_to(transform, Point3(2,1,10)); Point3 actual = Pose3::transform_to(transform, Point3(2,1,10));
Point3 expected(-1,2,10); Point3 expected(-1,2,10);
CHECK(assert_equal(expected, actual, 0.001)); CHECK(assert_equal(expected, actual, 0.001));
} }
@ -326,7 +326,7 @@ TEST( Pose3, transform_to_rotate)
TEST( Pose3, transform_to) TEST( Pose3, transform_to)
{ {
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0)); Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0));
Point3 actual = transform_to(transform, Point3(3,2,10)); Point3 actual = Pose3::transform_to(transform, Point3(3,2,10));
Point3 expected(2,1,10); Point3 expected(2,1,10);
CHECK(assert_equal(expected, actual, 0.001)); CHECK(assert_equal(expected, actual, 0.001));
} }
@ -334,7 +334,7 @@ TEST( Pose3, transform_to)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_from) TEST( Pose3, transform_from)
{ {
Point3 actual = transform_from(T3, Point3()); Point3 actual = Pose3::transform_from(T3, Point3());
Point3 expected = Point3(1.,2.,3.); Point3 expected = Point3(1.,2.,3.);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
@ -342,7 +342,7 @@ TEST( Pose3, transform_from)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_roundtrip) TEST( Pose3, transform_roundtrip)
{ {
Point3 actual = transform_from(T3, transform_to(T3, Point3(12., -0.11,7.0))); Point3 actual = Pose3::transform_from(T3, Pose3::transform_to(T3, Point3(12., -0.11,7.0)));
Point3 expected(12., -0.11,7.0); Point3 expected(12., -0.11,7.0);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }

4
geometry/tests/testRot2.cpp
View File

@ -66,7 +66,7 @@ inline Point2 rotate_(const Rot2 & R, const Point2& p) {return R.rotate(p);}
TEST( Rot2, rotate) TEST( Rot2, rotate)
{ {
Matrix H1, H2; Matrix H1, H2;
Point2 actual = rotate(R, P, H1, H2); Point2 actual = Rot2::rotate(R, P, H1, H2);
CHECK(assert_equal(actual,R*P)); CHECK(assert_equal(actual,R*P));
Matrix numerical1 = numericalDerivative21(rotate_, R, P); Matrix numerical1 = numericalDerivative21(rotate_, R, P);
CHECK(assert_equal(numerical1,H1)); CHECK(assert_equal(numerical1,H1));
@ -80,7 +80,7 @@ inline Point2 unrotate_(const Rot2 & R, const Point2& p) {return R.unrotate(p);}
TEST( Rot2, unrotate) TEST( Rot2, unrotate)
{ {
Matrix H1, H2; Matrix H1, H2;
Point2 w = R * P, actual = unrotate(R, w, H1, H2); Point2 w = R * P, actual = Rot2::unrotate(R, w, H1, H2);
CHECK(assert_equal(actual,P)); CHECK(assert_equal(actual,P));
Matrix numerical1 = numericalDerivative21(unrotate_, R, w); Matrix numerical1 = numericalDerivative21(unrotate_, R, w);
CHECK(assert_equal(numerical1,H1)); CHECK(assert_equal(numerical1,H1));

18
geometry/tests/testRot3.cpp
View File

@ -207,7 +207,7 @@ public:
}; };
AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
return cross(X, Y); return AngularVelocity::cross(X, Y);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -227,11 +227,11 @@ TEST(Rot3, BCH)
TEST( Rot3, rotate_derivatives) TEST( Rot3, rotate_derivatives)
{ {
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2; Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
rotate(R, P, actualDrotate1a, actualDrotate2); Rot3::rotate(R, P, actualDrotate1a, actualDrotate2);
rotate(R.inverse(), P, actualDrotate1b, boost::none); Rot3::rotate(R.inverse(), P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(rotate, R, P); Matrix numerical1 = numericalDerivative21(Rot3::rotate, R, P);
Matrix numerical2 = numericalDerivative21(rotate, R.inverse(), P); Matrix numerical2 = numericalDerivative21(Rot3::rotate, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(rotate, R, P); Matrix numerical3 = numericalDerivative22(Rot3::rotate, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error)); EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error)); EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error)); EXPECT(assert_equal(numerical3,actualDrotate2, error));
@ -242,13 +242,13 @@ TEST( Rot3, unrotate)
{ {
Point3 w = R * P; Point3 w = R * P;
Matrix H1,H2; Matrix H1,H2;
Point3 actual = unrotate(R,w,H1,H2); Point3 actual = Rot3::unrotate(R,w,H1,H2);
CHECK(assert_equal(P,actual)); CHECK(assert_equal(P,actual));
Matrix numerical1 = numericalDerivative21(unrotate, R, w); Matrix numerical1 = numericalDerivative21(Rot3::unrotate, R, w);
CHECK(assert_equal(numerical1,H1,error)); CHECK(assert_equal(numerical1,H1,error));
Matrix numerical2 = numericalDerivative22(unrotate, R, w); Matrix numerical2 = numericalDerivative22(Rot3::unrotate, R, w);
CHECK(assert_equal(numerical2,H2,error)); CHECK(assert_equal(numerical2,H2,error));
} }

6
slam/TransformConstraint.h
View File

@ -48,19 +48,19 @@ public:
boost::optional<Matrix&> Dlocal = boost::none) const { boost::optional<Matrix&> Dlocal = boost::none) const {
if (Dforeign) { if (Dforeign) {
Matrix Af; Matrix Af;
transform_from(trans, foreign, boost::none, Af); Transform::transform_from(trans, foreign, boost::none, Af);
*Dforeign = Af; *Dforeign = Af;
} }
if (Dtrans) { if (Dtrans) {
Matrix At; Matrix At;
transform_from(trans, foreign, At, boost::none); Transform::transform_from(trans, foreign, At, boost::none);
*Dtrans = At; *Dtrans = At;
} }
if (Dlocal) { if (Dlocal) {
Point dummy; Point dummy;
*Dlocal = -1* eye(dummy.dim()); *Dlocal = -1* eye(dummy.dim());
} }
return gtsam::logmap(gtsam::between<Point>(local, transform_from(trans, foreign))); return gtsam::logmap(gtsam::between<Point>(local, Transform::transform_from(trans, foreign)));
} }
}; };
} // \namespace gtsam } // \namespace gtsam

10
tests/testTransformConstraint.cpp
View File

@ -94,7 +94,7 @@ TEST( TransformConstraint, jacobians_zero ) {
// get values that are ideal // get values that are ideal
Pose2 trans(2.0, 3.0, 0.0); Pose2 trans(2.0, 3.0, 0.0);
Point2 global(5.0, 6.0); Point2 global(5.0, 6.0);
Point2 local = transform_from(trans, global); Point2 local = Pose2::transform_from(trans, global);
PointTransformConstraint tc(lA1, t1, lB1); PointTransformConstraint tc(lA1, t1, lB1);
Vector actCost = tc.evaluateError(global, trans, local), Vector actCost = tc.evaluateError(global, trans, local),
@ -129,8 +129,8 @@ TEST( TransformConstraint, converge_trans ) {
Pose2 transIdeal(7.0, 3.0, M_PI/2); Pose2 transIdeal(7.0, 3.0, M_PI/2);
// verify direction // verify direction
CHECK(assert_equal(local1, transform_from(transIdeal, global1))); CHECK(assert_equal(local1, Pose2::transform_from(transIdeal, global1)));
CHECK(assert_equal(local2, transform_from(transIdeal, global2))); CHECK(assert_equal(local2, Pose2::transform_from(transIdeal, global2)));
// choose transform // choose transform
// Pose2 trans = transIdeal; // ideal - works // Pose2 trans = transIdeal; // ideal - works
@ -187,7 +187,7 @@ TEST( TransformConstraint, converge_local ) {
// Pose2 trans(1.5, 2.5, 1.0); // larger rotation // Pose2 trans(1.5, 2.5, 1.0); // larger rotation
Pose2 trans(1.5, 2.5, 3.1); // significant rotation Pose2 trans(1.5, 2.5, 3.1); // significant rotation
Point2 idealLocal = transform_from(trans, global); Point2 idealLocal = Pose2::transform_from(trans, global);
// perturb the initial estimate // perturb the initial estimate
// Point2 local = idealLocal; // Ideal case - works // Point2 local = idealLocal; // Ideal case - works
@ -226,7 +226,7 @@ TEST( TransformConstraint, converge_global ) {
// Pose2 trans(1.5, 2.5, 1.0); // larger rotation // Pose2 trans(1.5, 2.5, 1.0); // larger rotation
Pose2 trans(1.5, 2.5, 3.1); // significant rotation Pose2 trans(1.5, 2.5, 3.1); // significant rotation
Point2 idealForeign = transform_from(inverse(trans), local); Point2 idealForeign = Pose2::transform_from(inverse(trans), local);
// perturb the initial estimate // perturb the initial estimate
// Point2 global = idealForeign; // Ideal - works // Point2 global = idealForeign; // Ideal - works