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 {
Point3 cameraPoint = transform_to(pose_, P);
Point3 cameraPoint = Pose3::transform_to(pose_, P);
Point2 intrinsic = project_to_camera(cameraPoint);
return intrinsic;
}
@ -67,7 +67,7 @@ namespace gtsam {
const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation();
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 d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
return Matrix_(2,6,
@ -80,7 +80,7 @@ namespace gtsam {
const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation();
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 d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
return Matrix_(2,3,
@ -95,7 +95,7 @@ namespace gtsam {
const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation();
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 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;

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;
}
/* ************************************************************************* */
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) {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = eye(3,3);
return add(p,q);
}
/* ************************************************************************* */
Point3 sub(const Point3 &p, const Point3 &q) {
Point3 Point3::sub(const Point3 &p, const Point3 &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) {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = -eye(3,3);
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_,
p.z_*q.x_ - p.x_*q.z_,
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_ );
}
/* ************************************************************************* */
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_ );
}

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));
}
/** 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);
/** add two points, add(p,q) is same as p+q */
static Point3 add (const Point3 &p, const Point3 &q);
static 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 */
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:
/** Serialization function */
@ -111,22 +130,4 @@ namespace gtsam {
/** Syntactic sugar for multiplying coordinates by a scalar s*p */
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
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) {
const Rot2& R = pose.r();
Point2 d = point - pose.t();
@ -134,7 +134,7 @@ namespace gtsam {
/* ************************************************************************* */
// 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) {
const Rot2& rot = pose.r();
const Point2 q = rot * p;
@ -181,14 +181,14 @@ namespace gtsam {
/* ************************************************************************* */
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);
}
Rot2 bearing(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
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;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * (*H1);
@ -198,14 +198,14 @@ namespace gtsam {
/* ************************************************************************* */
double range(const Pose2& pose, const Point2& point) {
Point2 d = transform_to(pose, point);
Point2 d = Pose2::transform_to(pose, point);
return d.norm();
}
double range(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
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);
Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
if (H1) *H1 = D_result_d * (*H1);

34
geometry/Pose2.h
View File

@ -94,6 +94,22 @@ namespace gtsam {
/** return transformation matrix */
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 */
inline double x() const { return t_.x(); }
inline double y() const { return t_.y(); }
@ -150,23 +166,9 @@ namespace gtsam {
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2 = boost::none);
/**
* 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);
/** syntactic sugar for transform_from */
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

22
geometry/Pose3.cpp
View File

@ -134,17 +134,17 @@ namespace gtsam {
/* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const {
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);
}
/* ************************************************************************* */
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();
}
/* ************************************************************************* */
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) {
if (H1) {
#ifdef CORRECT_POSE3_EXMAP
@ -153,22 +153,22 @@ namespace gtsam {
*H1 = collect(2,&DR,&R);
#else
Matrix DR;
rotate(pose.rotation(), p, DR, boost::none);
Rot3::rotate(pose.rotation(), p, DR, boost::none);
*H1 = collect(2,&DR,&I3);
#endif
}
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();
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) {
if (H1) { // *H1 = Dtransform_to1(pose, p);
Point3 q = transform_to(pose,p);
@ -182,7 +182,7 @@ namespace gtsam {
}
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 = collect(2, &DR_R1, &DR_t1);
Matrix Dt;
transform_from(p1,t2, Dt, boost::none);
Pose3::transform_from(p1,t2, Dt, boost::none);
*H1 = gtsam::stack(2, &DR, &Dt);
#endif
}
@ -226,7 +226,7 @@ namespace gtsam {
const Point3& t = p.translation();
Matrix Rt = R.transpose();
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 Dt = collect(2, &Dt_R1, &Dt_t1);
*H1 = gtsam::stack(2, &DR, &Dt);

22
geometry/Pose3.h
View File

@ -86,6 +86,16 @@ namespace gtsam {
/** composes two poses */
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
* rotation (in canonical coordinates). */
static Pose3 Expmap(const Vector& v);
@ -110,16 +120,8 @@ namespace gtsam {
/** Logarithm map around another pose T1 */
Vector logmap(const Pose3& T1, const Pose3& T2);
/** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Pose3& pose, const Point3& 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);
/** syntactic sugar for transform */
inline Point3 operator*(const Pose3& pose, const Point3& p) { return Pose3::transform_from(pose, p); }
/**
* Derivatives of compose

4
geometry/Rot2.cpp
View File

@ -74,7 +74,7 @@ namespace gtsam {
/* ************************************************************************* */
// 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) {
Point2 q = R * p;
if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x());
@ -84,7 +84,7 @@ namespace gtsam {
/* ************************************************************************* */
// 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) {
Point2 q = R.unrotate(p);
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 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 */
static Rot2 fromDegrees(double theta) {
@ -134,9 +136,23 @@ namespace gtsam {
/** rotate from world to rotated = R*p */
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 */
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:
/** Serialization function */
friend class boost::serialization::access;
@ -148,27 +164,8 @@ namespace gtsam {
}; // Rot2
/* inline named constructor implementation */
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
*/
/** syntactic sugar for rotate */
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

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) {
if (H1) *H1 = R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
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());
return Rt*p.vector(); // q = Rt*p
}
/* ************************************************************************* */
// 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) {
const Matrix Rt(R.transpose());
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();}
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:
/** Serialization function */
friend class boost::serialization::access;
@ -195,22 +211,6 @@ namespace gtsam {
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
*/

4
geometry/SimpleCamera.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
}
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 projection = uncalibrate(K_, intrinsic);
return pair<Point2, bool>(projection, cameraPoint.z() > 0);
@ -41,7 +41,7 @@ namespace gtsam {
Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_.calibrate(projection);
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) {

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 {
Point3 cameraPoint = transform_to(leftCamPose_, point);
Point3 cameraPoint = Pose3::transform_to(leftCamPose_, point);
double d = 1.0 / cameraPoint.z();
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);
//**** above function call inlined
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;
//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);
//**** above function call inlined
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
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)
{
CHECK(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(0,0,0),Point3(1,1,0)) == 0);
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
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(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(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));
}
/* ************************************************************************* */
Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
return transform_from(pose, point);
}
TEST (Pose2, transform_from)
{
Pose2 pose(1., 0., M_PI_2);
Point2 pt(2., 1.);
Matrix H1, H2;
Point2 actual = transform_from(pose, pt, H1, H2);
Point2 actual = Pose2::transform_from(pose, pt, H1, H2);
Point2 expected(0., 2.);
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 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, 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, numericalH2));
}
@ -197,8 +193,8 @@ TEST(Pose2, compose_a)
Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
Point2 expected_point(-1.0, -1.0);
Point2 actual_point1 = transform_to(pose1 * pose2, point);
Point2 actual_point2 = transform_to(pose2, transform_to(pose1, point));
Point2 actual_point1 = Pose2::transform_to(pose1 * pose2, 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_point2));
}

50
geometry/tests/testPose3.cpp
View File

@ -219,8 +219,8 @@ TEST( Pose3, compose_inverse)
TEST( Pose3, Dtransform_from1_a)
{
Matrix actualDtransform_from1;
transform_from(T, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from,T,P);
Pose3::transform_from(T, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(Pose3::transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
@ -228,8 +228,8 @@ TEST( Pose3, Dtransform_from1_b)
{
Pose3 origin;
Matrix actualDtransform_from1;
transform_from(origin, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from,origin,P);
Pose3::transform_from(origin, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(Pose3::transform_from,origin,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
@ -238,8 +238,8 @@ TEST( Pose3, Dtransform_from1_c)
Point3 origin;
Pose3 T0(R,origin);
Matrix actualDtransform_from1;
transform_from(T0, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from,T0,P);
Pose3::transform_from(T0, P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(Pose3::transform_from,T0,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
@ -249,9 +249,9 @@ TEST( Pose3, Dtransform_from1_d)
Point3 t0(100,0,0);
Pose3 T0(I,t0);
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:");
Matrix numerical = numericalDerivative21(transform_from,T0,P);
Matrix numerical = numericalDerivative21(Pose3::transform_from,T0,P);
//print(numerical, "Dtransform_from1_d numerical:");
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
}
@ -260,8 +260,8 @@ TEST( Pose3, Dtransform_from1_d)
TEST( Pose3, Dtransform_from2)
{
Matrix actualDtransform_from2;
transform_from(T,P, boost::none, actualDtransform_from2);
Matrix numerical = numericalDerivative22(transform_from,T,P);
Pose3::transform_from(T,P, boost::none, actualDtransform_from2);
Matrix numerical = numericalDerivative22(Pose3::transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from2,1e-8));
}
@ -269,8 +269,8 @@ TEST( Pose3, Dtransform_from2)
TEST( Pose3, Dtransform_to1)
{
Matrix computed;
transform_to(T, P, computed, boost::none);
Matrix numerical = numericalDerivative21(transform_to,T,P);
Pose3::transform_to(T, P, computed, boost::none);
Matrix numerical = numericalDerivative21(Pose3::transform_to,T,P);
CHECK(assert_equal(numerical,computed,1e-8));
}
@ -278,8 +278,8 @@ TEST( Pose3, Dtransform_to1)
TEST( Pose3, Dtransform_to2)
{
Matrix computed;
transform_to(T,P, boost::none, computed);
Matrix numerical = numericalDerivative22(transform_to,T,P);
Pose3::transform_to(T,P, boost::none, computed);
Matrix numerical = numericalDerivative22(Pose3::transform_to,T,P);
CHECK(assert_equal(numerical,computed,1e-8));
}
@ -287,9 +287,9 @@ TEST( Pose3, Dtransform_to2)
TEST( Pose3, transform_to_with_derivatives)
{
Matrix actH1, actH2;
transform_to(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_to, T,P),
expH2 = numericalDerivative22(transform_to, T,P);
Pose3::transform_to(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(Pose3::transform_to, T,P),
expH2 = numericalDerivative22(Pose3::transform_to, T,P);
CHECK(assert_equal(expH1, actH1, 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)
{
Matrix actH1, actH2;
transform_from(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_from, T,P),
expH2 = numericalDerivative22(transform_from, T,P);
Pose3::transform_from(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(Pose3::transform_from, T,P),
expH2 = numericalDerivative22(Pose3::transform_from, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8));
}
@ -308,7 +308,7 @@ TEST( Pose3, transform_from_with_derivatives)
/* ************************************************************************* */
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.);
CHECK(assert_equal(expected, actual));
}
@ -317,7 +317,7 @@ TEST( Pose3, transform_to_translate)
TEST( Pose3, transform_to_rotate)
{
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);
CHECK(assert_equal(expected, actual, 0.001));
}
@ -326,7 +326,7 @@ TEST( Pose3, transform_to_rotate)
TEST( Pose3, transform_to)
{
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);
CHECK(assert_equal(expected, actual, 0.001));
}
@ -334,7 +334,7 @@ TEST( Pose3, transform_to)
/* ************************************************************************* */
TEST( Pose3, transform_from)
{
Point3 actual = transform_from(T3, Point3());
Point3 actual = Pose3::transform_from(T3, Point3());
Point3 expected = Point3(1.,2.,3.);
CHECK(assert_equal(expected, actual));
}
@ -342,7 +342,7 @@ TEST( Pose3, transform_from)
/* ************************************************************************* */
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);
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)
{
Matrix H1, H2;
Point2 actual = rotate(R, P, H1, H2);
Point2 actual = Rot2::rotate(R, P, H1, H2);
CHECK(assert_equal(actual,R*P));
Matrix numerical1 = numericalDerivative21(rotate_, R, P);
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)
{
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));
Matrix numerical1 = numericalDerivative21(unrotate_, R, w);
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) {
return cross(X, Y);
return AngularVelocity::cross(X, Y);
}
/* ************************************************************************* */
@ -227,11 +227,11 @@ TEST(Rot3, BCH)
TEST( Rot3, rotate_derivatives)
{
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
rotate(R, P, actualDrotate1a, actualDrotate2);
rotate(R.inverse(), P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(rotate, R, P);
Matrix numerical2 = numericalDerivative21(rotate, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(rotate, R, P);
Rot3::rotate(R, P, actualDrotate1a, actualDrotate2);
Rot3::rotate(R.inverse(), P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(Rot3::rotate, R, P);
Matrix numerical2 = numericalDerivative21(Rot3::rotate, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(Rot3::rotate, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error));
@ -242,13 +242,13 @@ TEST( Rot3, unrotate)
{
Point3 w = R * P;
Matrix H1,H2;
Point3 actual = unrotate(R,w,H1,H2);
Point3 actual = Rot3::unrotate(R,w,H1,H2);
CHECK(assert_equal(P,actual));
Matrix numerical1 = numericalDerivative21(unrotate, R, w);
Matrix numerical1 = numericalDerivative21(Rot3::unrotate, R, w);
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));
}

6
slam/TransformConstraint.h
View File

@ -48,19 +48,19 @@ public:
boost::optional<Matrix&> Dlocal = boost::none) const {
if (Dforeign) {
Matrix Af;
transform_from(trans, foreign, boost::none, Af);
Transform::transform_from(trans, foreign, boost::none, Af);
*Dforeign = Af;
}
if (Dtrans) {
Matrix At;
transform_from(trans, foreign, At, boost::none);
Transform::transform_from(trans, foreign, At, boost::none);
*Dtrans = At;
}
if (Dlocal) {
Point dummy;
*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

10
tests/testTransformConstraint.cpp
View File

@ -94,7 +94,7 @@ TEST( TransformConstraint, jacobians_zero ) {
// get values that are ideal
Pose2 trans(2.0, 3.0, 0.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);
Vector actCost = tc.evaluateError(global, trans, local),
@ -129,8 +129,8 @@ TEST( TransformConstraint, converge_trans ) {
Pose2 transIdeal(7.0, 3.0, M_PI/2);
// verify direction
CHECK(assert_equal(local1, transform_from(transIdeal, global1)));
CHECK(assert_equal(local2, transform_from(transIdeal, global2)));
CHECK(assert_equal(local1, Pose2::transform_from(transIdeal, global1)));
CHECK(assert_equal(local2, Pose2::transform_from(transIdeal, global2)));
// choose transform
// 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, 3.1); // significant rotation
Point2 idealLocal = transform_from(trans, global);
Point2 idealLocal = Pose2::transform_from(trans, global);
// perturb the initial estimate
// 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, 3.1); // significant rotation
Point2 idealForeign = transform_from(inverse(trans), local);
Point2 idealForeign = Pose2::transform_from(inverse(trans), local);
// perturb the initial estimate
// Point2 global = idealForeign; // Ideal - works