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

Pose3 naming convention

release/4.3a0
Frank Dellaert 2019-05-16 14:33:58 -04:00
parent ae1a096239
commit 8801de4d63
52 changed files with 318 additions and 278 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cython/gtsam/tests/test_Pose3.py
View File

@ -30,9 +30,9 @@ class TestPose3(GtsamTestCase):
self.gtsamAssertEquals(actual, expected, 1e-6) self.gtsamAssertEquals(actual, expected, 1e-6)
def test_transform_to(self): def test_transform_to(self):
"""Test transform_to method.""" """Test transformTo method."""
transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
actual = transform.transform_to(Point3(3, 2, 10)) actual = transform.transformTo(Point3(3, 2, 10))
expected = Point3(2, 1, 10) expected = Point3(2, 1, 10)
self.gtsamAssertEquals(actual, expected, 1e-6) self.gtsamAssertEquals(actual, expected, 1e-6)

2
examples/SFMExampleExpressions.cpp
View File

@ -72,7 +72,7 @@ int main(int argc, char* argv[]) {
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
// Below an expression for the prediction of the measurement: // Below an expression for the prediction of the measurement:
Point3_ p('l', j); Point3_ p('l', j);
Point2_ prediction = uncalibrate(cK, project(transform_to(x, p))); Point2_ prediction = uncalibrate(cK, project(transformTo(x, p)));
// Again, here we use an ExpressionFactor // Again, here we use an ExpressionFactor
graph.addExpressionFactor(prediction, measurement, measurementNoise); graph.addExpressionFactor(prediction, measurement, measurementNoise);
} }

2
examples/SolverComparer.cpp
View File

@ -362,7 +362,7 @@ void runIncremental()
Rot2 measuredBearing = measured.bearing(); Rot2 measuredBearing = measured.bearing();
double measuredRange = measured.range(); double measuredRange = measured.range();
newVariables.insert(lmKey, newVariables.insert(lmKey,
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); pose.transformFrom(measuredBearing.rotate(Point2(measuredRange, 0.0))));
} }
} }
else else

4
examples/StereoVOExample_large.cpp
View File

@ -89,8 +89,8 @@ int main(int argc, char** argv){
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) { if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x)); Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint); initial_estimate.insert(Symbol('l', l), worldPoint);
} }
} }

11
gtsam.h
View File

@ -632,8 +632,8 @@ class Pose2 {
static Matrix wedge(double vx, double vy, double w); static Matrix wedge(double vx, double vy, double w);
// Group Actions on Point2 // Group Actions on Point2
gtsam::Point2 transform_from(const gtsam::Point2& p) const; gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
gtsam::Point2 transform_to(const gtsam::Point2& p) const; gtsam::Point2 transformTo(const gtsam::Point2& p) const;
// Standard Interface // Standard Interface
double x() const; double x() const;
@ -685,8 +685,8 @@ class Pose3 {
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
// Group Action on Point3 // Group Action on Point3
gtsam::Point3 transform_from(const gtsam::Point3& point) const; gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
gtsam::Point3 transform_to(const gtsam::Point3& point) const; gtsam::Point3 transformTo(const gtsam::Point3& point) const;
// Standard Interface // Standard Interface
gtsam::Rot3 rotation() const; gtsam::Rot3 rotation() const;
@ -695,7 +695,8 @@ class Pose3 {
double y() const; double y() const;
double z() const; double z() const;
Matrix matrix() const; Matrix matrix() const;
gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const;
gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const;
double range(const gtsam::Point3& point); double range(const gtsam::Point3& point);
double range(const gtsam::Pose3& pose); double range(const gtsam::Pose3& pose);

6
gtsam/geometry/CalibratedCamera.cpp
View File

@ -107,7 +107,7 @@ Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
/* ************************************************************************* */ /* ************************************************************************* */
pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const { pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw); const Point3 pc = pose().transformTo(pw);
const Point2 pn = Project(pc); const Point2 pn = Project(pc);
return make_pair(pn, pc.z() > 0); return make_pair(pn, pc.z() > 0);
} }
@ -116,8 +116,8 @@ pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const { OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transform_to if needed Matrix3 Rt; // calculated by transformTo if needed
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); const Point3 q = pose().transformTo(point, boost::none, Dpoint ? &Rt : 0);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0) if (q.z() <= 0)
throw CheiralityException(); throw CheiralityException();

2
gtsam/geometry/CalibratedCamera.h
View File

@ -351,7 +351,7 @@ public:
Dresult_ddepth ? &Dpoint_ddepth : 0); Dresult_ddepth ? &Dpoint_ddepth : 0);
Matrix33 Dresult_dpoint; Matrix33 Dresult_dpoint;
const Point3 result = pose().transform_from(point, Dresult_dpose, const Point3 result = pose().transformFrom(point, Dresult_dpose,
(Dresult_ddepth || (Dresult_ddepth ||
Dresult_dp) ? &Dresult_dpoint : 0); Dresult_dp) ? &Dresult_dpoint : 0);

6
gtsam/geometry/EssentialMatrix.cpp
View File

@ -52,13 +52,13 @@ void EssentialMatrix::print(const string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, Point3 EssentialMatrix::transformTo(const Point3& p, OptionalJacobian<3, 5> DE,
OptionalJacobian<3, 3> Dpoint) const { OptionalJacobian<3, 3> Dpoint) const {
Pose3 pose(rotation(), direction().point3()); Pose3 pose(rotation(), direction().point3());
Matrix36 DE_; Matrix36 DE_;
Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); Point3 q = pose.transformTo(p, DE ? &DE_ : 0, Dpoint);
if (DE) { if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5 // DE returned by pose.transformTo is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation // The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis() // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis()
// Duy made an educated guess that this needs to be rotated to the local frame // Duy made an educated guess that this needs to be rotated to the local frame

13
gtsam/geometry/EssentialMatrix.h
View File

@ -138,7 +138,7 @@ class GTSAM_EXPORT EssentialMatrix {
* @param Dpoint optional 3*3 Jacobian wrpt point * @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in pose coordinates * @return point in pose coordinates
*/ */
Point3 transform_to(const Point3& p, Point3 transformTo(const Point3& p,
OptionalJacobian<3, 5> DE = boost::none, OptionalJacobian<3, 5> DE = boost::none,
OptionalJacobian<3, 3> Dpoint = boost::none) const; OptionalJacobian<3, 3> Dpoint = boost::none) const;
@ -176,6 +176,17 @@ class GTSAM_EXPORT EssentialMatrix {
/// @} /// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point3 transform_to(const Point3& p,
OptionalJacobian<3, 5> DE = boost::none,
OptionalJacobian<3, 3> Dpoint = boost::none) const {
return transformTo(p, DE, Dpoint);
};
/// @}
#endif
private: private:
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{

2
gtsam/geometry/PinholePose.h
View File

@ -145,7 +145,7 @@ public:
(Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0, (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
Dresult_ddepth ? &Dpoint_ddepth : 0); Dresult_ddepth ? &Dpoint_ddepth : 0);
Matrix33 Dresult_dpoint; Matrix33 Dresult_dpoint;
const Point3 result = pose().transform_from(point, Dresult_dpose, const Point3 result = pose().transformFrom(point, Dresult_dpose,
(Dresult_ddepth || (Dresult_ddepth ||
Dresult_dp || Dresult_dp ||
Dresult_dcal) ? &Dresult_dpoint : 0); Dresult_dcal) ? &Dresult_dpoint : 0);

8
gtsam/geometry/Pose2.cpp
View File

@ -198,7 +198,7 @@ Pose2 Pose2::inverse() const {
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SE(2) section // see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point, Point2 Pose2::transformTo(const Point2& point,
OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const { OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const {
OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2); OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
@ -209,7 +209,7 @@ Point2 Pose2::transform_to(const Point2& point,
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SE(2) section // see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& point, Point2 Pose2::transformFrom(const Point2& point,
OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const { OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const {
OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2); OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
@ -223,7 +223,7 @@ Rot2 Pose2::bearing(const Point2& point,
OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const { OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const {
// make temporary matrices // make temporary matrices
Matrix23 D_d_pose; Matrix2 D_d_point; Matrix23 D_d_pose; Matrix2 D_d_point;
Point2 d = transform_to(point, Hpose ? &D_d_pose : 0, Hpoint ? &D_d_point : 0); Point2 d = transformTo(point, Hpose ? &D_d_pose : 0, Hpoint ? &D_d_point : 0);
if (!Hpose && !Hpoint) return Rot2::relativeBearing(d); if (!Hpose && !Hpoint) return Rot2::relativeBearing(d);
Matrix12 D_result_d; Matrix12 D_result_d;
Rot2 result = Rot2::relativeBearing(d, Hpose || Hpoint ? &D_result_d : 0); Rot2 result = Rot2::relativeBearing(d, Hpose || Hpoint ? &D_result_d : 0);
@ -288,7 +288,7 @@ double Pose2::range(const Pose2& pose,
/* ************************************************************************* /* *************************************************************************
* New explanation, from scan.ml * New explanation, from scan.ml
* It finds the angle using a linear method: * It finds the angle using a linear method:
* q = Pose2::transform_from(p) = t + R*p * q = Pose2::transformFrom(p) = t + R*p
* We need to remove the centroids from the data to find the rotation * We need to remove the centroids from the data to find the rotation
* using dp=[dpx;dpy] and q=[dqx;dqy] we have * using dp=[dpx;dpy] and q=[dqx;dqy] we have
* |dqx| |c -s| |dpx| |dpx -dpy| |c| * |dqx| |c -s| |dpx| |dpx -dpy| |c|

36
gtsam/geometry/Pose2.h
View File

@ -195,17 +195,17 @@ public:
/// @{ /// @{
/** Return point coordinates in pose coordinate frame */ /** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point, Point2 transformTo(const Point2& point,
OptionalJacobian<2, 3> H1 = boost::none, OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const; OptionalJacobian<2, 2> Dpoint = boost::none) const;
/** Return point coordinates in global frame */ /** Return point coordinates in global frame */
Point2 transform_from(const Point2& point, Point2 transformFrom(const Point2& point,
OptionalJacobian<2, 3> H1 = boost::none, OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const; OptionalJacobian<2, 2> Dpoint = boost::none) const;
/** syntactic sugar for transform_from */ /** syntactic sugar for transformFrom */
inline Point2 operator*(const Point2& point) const { return transform_from(point);} inline Point2 operator*(const Point2& point) const { return transformFrom(point);}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
@ -289,7 +289,23 @@ public:
/// @} /// @}
private: #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point2 transform_from(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
return transformFrom(point, Dpose, Dpoint);
}
Point2 transform_to(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
return transformTo(point, Dpose, Dpoint);
}
/// @}
#endif
private:
// Serialization function // Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
@ -313,7 +329,7 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
/** /**
* Calculate pose between a vector of 2D point correspondences (p,q) * Calculate pose between a vector of 2D point correspondences (p,q)
* where q = Pose2::transform_from(p) = t + R*p * where q = Pose2::transformFrom(p) = t + R*p
*/ */
typedef std::pair<Point2,Point2> Point2Pair; typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs); GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);

23
gtsam/geometry/Pose3.cpp
View File

@ -284,6 +284,12 @@ Matrix4 Pose3::matrix() const {
return mat; return mat;
} }
/* ************************************************************************* */
Pose3 Pose3::transformPoseFrom(const Pose3& aTb) const {
const Pose3& wTa = *this;
return wTa * aTb;
}
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Pose3 Pose3::transform_to(const Pose3& pose) const { Pose3 Pose3::transform_to(const Pose3& pose) const {
@ -294,15 +300,16 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
#endif #endif
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1, Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> H1,
OptionalJacobian<6, 6> H2) const { OptionalJacobian<6, 6> H2) const {
if (H1) *H1 = -pose.inverse().AdjointMap() * AdjointMap(); if (H1) *H1 = -wTb.inverse().AdjointMap() * AdjointMap();
if (H2) *H2 = I_6x6; if (H2) *H2 = I_6x6;
return inverse() * pose; const Pose3& wTa = *this;
return wTa.inverse() * wTb;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const { OptionalJacobian<3,3> Dpoint) const {
// Only get matrix once, to avoid multiple allocations, // Only get matrix once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case // as well as multiple conversions in the Quaternion case
@ -318,7 +325,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const { OptionalJacobian<3,3> Dpoint) const {
// Only get transpose once, to avoid multiple allocations, // Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case // as well as multiple conversions in the Quaternion case
@ -342,7 +349,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 3> H2) const { OptionalJacobian<1, 3> H2) const {
Matrix36 D_local_pose; Matrix36 D_local_pose;
Matrix3 D_local_point; Matrix3 D_local_point;
Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
if (!H1 && !H2) { if (!H1 && !H2) {
return local.norm(); return local.norm();
} else { } else {
@ -368,7 +375,7 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
OptionalJacobian<2, 3> H2) const { OptionalJacobian<2, 3> H2) const {
Matrix36 D_local_pose; Matrix36 D_local_pose;
Matrix3 D_local_point; Matrix3 D_local_point;
Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
if (!H1 && !H2) { if (!H1 && !H2) {
return Unit3(local); return Unit3(local);
} else { } else {

42
gtsam/geometry/Pose3.h
View File

@ -81,7 +81,6 @@ public:
/** /**
* Create Pose3 by aligning two point pairs * Create Pose3 by aligning two point pairs
* A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
* Meant to replace the deprecated function 'align', which orders the pairs the opposite way.
* Note this allows for noise on the points but in that case the mapping will not be exact. * Note this allows for noise on the points but in that case the mapping will not be exact.
*/ */
static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs); static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
@ -207,12 +206,12 @@ public:
* @param Dpoint optional 3*3 Jacobian wrpt point * @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in world coordinates * @return point in world coordinates
*/ */
Point3 transform_from(const Point3& p, OptionalJacobian<3, 6> Dpose = Point3 transformFrom(const Point3& p, OptionalJacobian<3, 6> Dpose =
boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
/** syntactic sugar for transform_from */ /** syntactic sugar for transformFrom */
inline Point3 operator*(const Point3& p) const { inline Point3 operator*(const Point3& p) const {
return transform_from(p); return transformFrom(p);
} }
/** /**
@ -222,7 +221,7 @@ public:
* @param Dpoint optional 3*3 Jacobian wrpt point * @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates * @return point in Pose coordinates
*/ */
Point3 transform_to(const Point3& p, OptionalJacobian<3, 6> Dpose = Point3 transformTo(const Point3& p, OptionalJacobian<3, 6> Dpose =
boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
/// @} /// @}
@ -253,9 +252,12 @@ public:
/** convert to 4*4 matrix */ /** convert to 4*4 matrix */
Matrix4 matrix() const; Matrix4 matrix() const;
/** receives a pose in local coordinates and transforms it to world coordinates */
Pose3 transformPoseFrom(const Pose3& pose) const;
/** receives a pose in world coordinates and transforms it to local coordinates */ /** receives a pose in world coordinates and transforms it to local coordinates */
Pose3 transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const; OptionalJacobian<6, 6> H2 = boost::none) const;
/** /**
* Calculate range to a landmark * Calculate range to a landmark
@ -319,7 +321,23 @@ public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated /// @name Deprecated
/// @{ /// @{
/// This function is neither here not there. Do not use. Point3 transform_from(const Point3& p,
OptionalJacobian<3, 6> Dpose = boost::none,
OptionalJacobian<3, 3> Dpoint = boost::none) const {
return transformFrom(p, Dpose, Dpoint);
}
Point3 transform_to(const Point3& p,
OptionalJacobian<3, 6> Dpose = boost::none,
OptionalJacobian<3, 3> Dpoint = boost::none) const {
return transformTo(p, Dpose, Dpoint);
}
Pose3 transform_pose_to(const Pose3& pose,
OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const {
return transformPoseTo(pose, H1, H2);
}
/**
* @deprecated: this function is neither here not there. */
Pose3 transform_to(const Pose3& pose) const; Pose3 transform_to(const Pose3& pose) const;
/// @} /// @}
#endif #endif
@ -354,12 +372,10 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
} }
/** #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
* Calculate pose between a vector of 3D point correspondences (b_point, a_point) // deprecated: use Pose3::Align with point pairs ordered the opposite way
* where a_point = Pose3::transform_from(b_point) = t + R*b_point
* @deprecated: use Pose3::Align with point pairs ordered the opposite way
*/
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& baPointPairs); GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& baPointPairs);
#endif
// For MATLAB wrapper // For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector; typedef std::vector<Pose3> Pose3Vector;

8
gtsam/geometry/StereoCamera.cpp
View File

@ -37,7 +37,7 @@ namespace gtsam {
StereoPoint2 StereoCamera::project2(const Point3& point, StereoPoint2 StereoCamera::project2(const Point3& point,
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const {
const Point3 q = leftCamPose_.transform_to(point); const Point3 q = leftCamPose_.transformTo(point);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0) if (q.z() <= 0)
@ -94,7 +94,7 @@ namespace gtsam {
double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
double X = Z * (measured[0] - K_->px()) / K_->fx(); double X = Z * (measured[0] - K_->px()) / K_->fx();
double Y = Z * (measured[2] - K_->py()) / K_->fy(); double Y = Z * (measured[2] - K_->py()) / K_->fy();
Point3 point = leftCamPose_.transform_from(Point3(X, Y, Z)); Point3 point = leftCamPose_.transformFrom(Point3(X, Y, Z));
return point; return point;
} }
@ -120,7 +120,7 @@ namespace gtsam {
-z_partial_uR, z_partial_uR, 0; -z_partial_uR, z_partial_uR, 0;
Matrix3 D_point_local; Matrix3 D_point_local;
const Point3 point = leftCamPose_.transform_from(local, H1, D_point_local); const Point3 point = leftCamPose_.transformFrom(local, H1, D_point_local);
if(H2) { if(H2) {
*H2 = D_point_local * D_local_z; *H2 = D_point_local * D_local_z;
@ -129,7 +129,7 @@ namespace gtsam {
return point; return point;
} }
return leftCamPose_.transform_from(local); return leftCamPose_.transformFrom(local);
} }
} }

2
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -134,7 +134,7 @@ TEST( CalibratedCamera, Dproject_point_pose)
Point2 result = camera.project(point1, Dpose, Dpoint); Point2 result = camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, camera, point1); Matrix numerical_pose = numericalDerivative21(project2, camera, point1);
Matrix numerical_point = numericalDerivative22(project2, camera, point1); Matrix numerical_point = numericalDerivative22(project2, camera, point1);
CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1))); CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transformTo(point1)));
CHECK(assert_equal(Point2(-.16, .16), result)); CHECK(assert_equal(Point2(-.16, .16), result));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7));

6
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -115,9 +115,9 @@ TEST (EssentialMatrix, RoundTrip) {
//************************************************************************* //*************************************************************************
Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
return E.transform_to(point); return E.transformTo(point);
} }
TEST (EssentialMatrix, transform_to) { TEST (EssentialMatrix, transformTo) {
// test with a more complicated EssentialMatrix // test with a more complicated EssentialMatrix
Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4) Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4)
* Rot3::Roll(M_PI / 6.0); * Rot3::Roll(M_PI / 6.0);
@ -126,7 +126,7 @@ TEST (EssentialMatrix, transform_to) {
//EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0))); //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0)));
static Point3 P(0.2, 0.7, -2); static Point3 P(0.2, 0.7, -2);
Matrix actH1, actH2; Matrix actH1, actH2;
E.transform_to(P, actH1, actH2); E.transformTo(P, actH1, actH2);
Matrix expH1 = numericalDerivative21(transform_to_, E, P), // Matrix expH1 = numericalDerivative21(transform_to_, E, P), //
expH2 = numericalDerivative22(transform_to_, E, P); expH2 = numericalDerivative22(transform_to_, E, P);
EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH1, actH1, 1e-8));

57
gtsam/geometry/tests/testPose2.cpp
View File

@ -266,45 +266,44 @@ TEST( Pose2, LogmapDerivative2) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Point2 transform_to_proxy(const Pose2& pose, const Point2& point) { static Point2 transformTo_(const Pose2& pose, const Point2& point) {
return pose.transform_to(point); return pose.transformTo(point);
} }
TEST( Pose2, transform_to ) TEST(Pose2, transformTo) {
{ Pose2 pose(M_PI / 2.0, Point2(1, 2)); // robot at (1,2) looking towards y
Pose2 pose(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y Point2 point(-1, 4); // landmark at (-1,4)
Point2 point(-1,4); // landmark at (-1,4)
// expected // expected
Point2 expected(2,2); Point2 expected(2, 2);
Matrix expectedH1 = (Matrix(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0).finished(); Matrix expectedH1 =
Matrix expectedH2 = (Matrix(2,2) << 0.0, 1.0, -1.0, 0.0).finished(); (Matrix(2, 3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0).finished();
Matrix expectedH2 = (Matrix(2, 2) << 0.0, 1.0, -1.0, 0.0).finished();
// actual // actual
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
Point2 actual = pose.transform_to(point, actualH1, actualH2); Point2 actual = pose.transformTo(point, actualH1, actualH2);
EXPECT(assert_equal(expected,actual)); EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1, actualH1));
Matrix numericalH1 = numericalDerivative21(transform_to_proxy, pose, point); Matrix numericalH1 = numericalDerivative21(transformTo_, pose, point);
EXPECT(assert_equal(numericalH1,actualH1)); EXPECT(assert_equal(numericalH1, actualH1));
EXPECT(assert_equal(expectedH2,actualH2)); EXPECT(assert_equal(expectedH2, actualH2));
Matrix numericalH2 = numericalDerivative22(transform_to_proxy, pose, point); Matrix numericalH2 = numericalDerivative22(transformTo_, pose, point);
EXPECT(assert_equal(numericalH2,actualH2)); EXPECT(assert_equal(numericalH2, actualH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Point2 transform_from_proxy(const Pose2& pose, const Point2& point) { static Point2 transformFrom_(const Pose2& pose, const Point2& point) {
return pose.transform_from(point); return pose.transformFrom(point);
} }
TEST (Pose2, transform_from) TEST(Pose2, transformFrom) {
{ Pose2 pose(1., 0., M_PI / 2.0);
Pose2 pose(1., 0., M_PI/2.0);
Point2 pt(2., 1.); Point2 pt(2., 1.);
Matrix H1, H2; Matrix H1, H2;
Point2 actual = pose.transform_from(pt, H1, H2); Point2 actual = pose.transformFrom(pt, H1, H2);
Point2 expected(0., 2.); Point2 expected(0., 2.);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -312,11 +311,11 @@ TEST (Pose2, transform_from)
Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.).finished(); Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.).finished();
Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.).finished(); Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.).finished();
Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt); Matrix numericalH1 = numericalDerivative21(transformFrom_, pose, pt);
EXPECT(assert_equal(H1_expected, H1)); EXPECT(assert_equal(H1_expected, H1));
EXPECT(assert_equal(H1_expected, numericalH1)); EXPECT(assert_equal(H1_expected, numericalH1));
Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt); Matrix numericalH2 = numericalDerivative22(transformFrom_, pose, pt);
EXPECT(assert_equal(H2_expected, H2)); EXPECT(assert_equal(H2_expected, H2));
EXPECT(assert_equal(H2_expected, numericalH2)); EXPECT(assert_equal(H2_expected, numericalH2));
} }
@ -349,8 +348,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 = (pose1 * pose2).transform_to(point); Point2 actual_point1 = (pose1 * pose2).transformTo(point);
Point2 actual_point2 = pose2.transform_to(pose1.transform_to(point)); Point2 actual_point2 = pose2.transformTo(pose1.transformTo(point));
EXPECT(assert_equal(expected_point, actual_point1)); EXPECT(assert_equal(expected_point, actual_point1));
EXPECT(assert_equal(expected_point, actual_point2)); EXPECT(assert_equal(expected_point, actual_point2));
} }
@ -735,7 +734,7 @@ TEST(Pose2, align_2) {
vector<Point2Pair> correspondences; vector<Point2Pair> correspondences;
Point2 p1(0,0), p2(10,0); Point2 p1(0,0), p2(10,0);
Point2 q1 = expected.transform_from(p1), q2 = expected.transform_from(p2); Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2);
EXPECT(assert_equal(Point2(20,10),q1)); EXPECT(assert_equal(Point2(20,10),q1));
EXPECT(assert_equal(Point2(20,20),q2)); EXPECT(assert_equal(Point2(20,20),q2));
Point2Pair pq1(make_pair(p1, q1)); Point2Pair pq1(make_pair(p1, q1));
@ -750,7 +749,7 @@ namespace align_3 {
Point2 t(10,10); Point2 t(10,10);
Pose2 expected(Rot2::fromAngle(2*M_PI/3), t); Pose2 expected(Rot2::fromAngle(2*M_PI/3), t);
Point2 p1(0,0), p2(10,0), p3(10,10); Point2 p1(0,0), p2(10,0), p3(10,10);
Point2 q1 = expected.transform_from(p1), q2 = expected.transform_from(p2), q3 = expected.transform_from(p3); Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2), q3 = expected.transformFrom(p3);
} }
TEST(Pose2, align_3) { TEST(Pose2, align_3) {

184
gtsam/geometry/tests/testPose3.cpp
View File

@ -316,172 +316,162 @@ TEST( Pose3, compose_inverse)
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 transform_from_(const Pose3& pose, const Point3& point) { return pose.transform_from(point); } Point3 transformFrom_(const Pose3& pose, const Point3& point) {
TEST( Pose3, Dtransform_from1_a) return pose.transformFrom(point);
{ }
TEST(Pose3, Dtransform_from1_a) {
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
T.transform_from(P, actualDtransform_from1, boost::none); T.transformFrom(P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from_,T,P); Matrix numerical = numericalDerivative21(transformFrom_, T, P);
EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
} }
TEST( Pose3, Dtransform_from1_b) TEST(Pose3, Dtransform_from1_b) {
{
Pose3 origin; Pose3 origin;
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
origin.transform_from(P, actualDtransform_from1, boost::none); origin.transformFrom(P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from_,origin,P); Matrix numerical = numericalDerivative21(transformFrom_, origin, P);
EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
} }
TEST( Pose3, Dtransform_from1_c) TEST(Pose3, Dtransform_from1_c) {
{ Point3 origin(0, 0, 0);
Point3 origin(0,0,0); Pose3 T0(R, origin);
Pose3 T0(R,origin);
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
T0.transform_from(P, actualDtransform_from1, boost::none); T0.transformFrom(P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from_,T0,P); Matrix numerical = numericalDerivative21(transformFrom_, T0, P);
EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
} }
TEST( Pose3, Dtransform_from1_d) TEST(Pose3, Dtransform_from1_d) {
{
Rot3 I; Rot3 I;
Point3 t0(100,0,0); Point3 t0(100, 0, 0);
Pose3 T0(I,t0); Pose3 T0(I, t0);
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
T0.transform_from(P, actualDtransform_from1, boost::none); T0.transformFrom(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(transformFrom_, T0, P);
//print(numerical, "Dtransform_from1_d numerical:"); // print(numerical, "Dtransform_from1_d numerical:");
EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, Dtransform_from2) TEST(Pose3, Dtransform_from2) {
{
Matrix actualDtransform_from2; Matrix actualDtransform_from2;
T.transform_from(P, boost::none, actualDtransform_from2); T.transformFrom(P, boost::none, actualDtransform_from2);
Matrix numerical = numericalDerivative22(transform_from_,T,P); Matrix numerical = numericalDerivative22(transformFrom_, T, P);
EXPECT(assert_equal(numerical,actualDtransform_from2,1e-8)); EXPECT(assert_equal(numerical, actualDtransform_from2, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 transform_to_(const Pose3& pose, const Point3& point) { return pose.transform_to(point); } Point3 transform_to_(const Pose3& pose, const Point3& point) {
TEST( Pose3, Dtransform_to1) return pose.transformTo(point);
{ }
TEST(Pose3, Dtransform_to1) {
Matrix computed; Matrix computed;
T.transform_to(P, computed, boost::none); T.transformTo(P, computed, boost::none);
Matrix numerical = numericalDerivative21(transform_to_,T,P); Matrix numerical = numericalDerivative21(transform_to_, T, P);
EXPECT(assert_equal(numerical,computed,1e-8)); EXPECT(assert_equal(numerical, computed, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, Dtransform_to2) TEST(Pose3, Dtransform_to2) {
{
Matrix computed; Matrix computed;
T.transform_to(P, boost::none, computed); T.transformTo(P, boost::none, computed);
Matrix numerical = numericalDerivative22(transform_to_,T,P); Matrix numerical = numericalDerivative22(transform_to_, T, P);
EXPECT(assert_equal(numerical,computed,1e-8)); EXPECT(assert_equal(numerical, computed, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_to_with_derivatives) TEST(Pose3, transform_to_with_derivatives) {
{
Matrix actH1, actH2; Matrix actH1, actH2;
T.transform_to(P,actH1,actH2); T.transformTo(P, actH1, actH2);
Matrix expH1 = numericalDerivative21(transform_to_, T,P), Matrix expH1 = numericalDerivative21(transform_to_, T, P),
expH2 = numericalDerivative22(transform_to_, T,P); expH2 = numericalDerivative22(transform_to_, T, P);
EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH1, actH1, 1e-8));
EXPECT(assert_equal(expH2, actH2, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_from_with_derivatives) TEST(Pose3, transform_from_with_derivatives) {
{
Matrix actH1, actH2; Matrix actH1, actH2;
T.transform_from(P,actH1,actH2); T.transformFrom(P, actH1, actH2);
Matrix expH1 = numericalDerivative21(transform_from_, T,P), Matrix expH1 = numericalDerivative21(transformFrom_, T, P),
expH2 = numericalDerivative22(transform_from_, T,P); expH2 = numericalDerivative22(transformFrom_, T, P);
EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH1, actH1, 1e-8));
EXPECT(assert_equal(expH2, actH2, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_to_translate) TEST(Pose3, transform_to_translate) {
{ Point3 actual =
Point3 actual = Pose3(Rot3(), Point3(1, 2, 3)).transform_to(Point3(10.,20.,30.)); Pose3(Rot3(), Point3(1, 2, 3)).transformTo(Point3(10., 20., 30.));
Point3 expected(9.,18.,27.); Point3 expected(9., 18., 27.);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_to_rotate) TEST(Pose3, transform_to_rotate) {
{ Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(0, 0, 0));
Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0)); Point3 actual = transform.transformTo(Point3(2, 1, 10));
Point3 actual = transform.transform_to(Point3(2,1,10)); Point3 expected(-1, 2, 10);
Point3 expected(-1,2,10); EXPECT(assert_equal(expected, actual, 0.001));
EXPECT(assert_equal(expected, actual, 0.001));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_to) TEST(Pose3, transformTo) {
{ Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(2, 4, 0));
Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(2,4, 0)); Point3 actual = transform.transformTo(Point3(3, 2, 10));
Point3 actual = transform.transform_to(Point3(3,2,10)); Point3 expected(2, 1, 10);
Point3 expected(2,1,10); EXPECT(assert_equal(expected, actual, 0.001));
EXPECT(assert_equal(expected, actual, 0.001));
} }
Pose3 transform_pose_to_(const Pose3& pose, const Pose3& pose2) { return pose.transform_pose_to(pose2); } Pose3 transformPoseTo_(const Pose3& pose, const Pose3& pose2) {
return pose.transformPoseTo(pose2);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_pose_to) TEST(Pose3, transformPoseTo) {
{ Pose3 origin = T.transformPoseTo(T);
Pose3 origin = T.transform_pose_to(T);
EXPECT(assert_equal(Pose3{}, origin)); EXPECT(assert_equal(Pose3{}, origin));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_pose_to_with_derivatives) TEST(Pose3, transformPoseTo_with_derivatives) {
{
Matrix actH1, actH2; Matrix actH1, actH2;
Pose3 res = T.transform_pose_to(T2,actH1,actH2); Pose3 res = T.transformPoseTo(T2, actH1, actH2);
EXPECT(assert_equal(res, T.inverse().compose(T2))); EXPECT(assert_equal(res, T.inverse().compose(T2)));
Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T2), Matrix expH1 = numericalDerivative21(transformPoseTo_, T, T2),
expH2 = numericalDerivative22(transform_pose_to_, T, T2); expH2 = numericalDerivative22(transformPoseTo_, T, T2);
EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH1, actH1, 1e-8));
EXPECT(assert_equal(expH2, actH2, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_pose_to_with_derivatives2) TEST(Pose3, transformPoseTo_with_derivatives2) {
{
Matrix actH1, actH2; Matrix actH1, actH2;
Pose3 res = T.transform_pose_to(T3,actH1,actH2); Pose3 res = T.transformPoseTo(T3, actH1, actH2);
EXPECT(assert_equal(res, T.inverse().compose(T3))); EXPECT(assert_equal(res, T.inverse().compose(T3)));
Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T3), Matrix expH1 = numericalDerivative21(transformPoseTo_, T, T3),
expH2 = numericalDerivative22(transform_pose_to_, T, T3); expH2 = numericalDerivative22(transformPoseTo_, T, T3);
EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH1, actH1, 1e-8));
EXPECT(assert_equal(expH2, actH2, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_from) TEST(Pose3, transformFrom) {
{ Point3 actual = T3.transformFrom(Point3(0, 0, 0));
Point3 actual = T3.transform_from(Point3(0,0,0)); Point3 expected = Point3(1., 2., 3.);
Point3 expected = Point3(1.,2.,3.); EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_roundtrip) TEST(Pose3, transform_roundtrip) {
{ Point3 actual = T3.transformFrom(T3.transformTo(Point3(12., -0.11, 7.0)));
Point3 actual = T3.transform_from(T3.transform_to(Point3(12., -0.11,7.0))); Point3 expected(12., -0.11, 7.0);
Point3 expected(12., -0.11,7.0); EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -737,9 +727,9 @@ TEST(Pose3, Align2) {
vector<Point3Pair> correspondences; vector<Point3Pair> correspondences;
Point3 p1(0,0,1), p2(10,0,2), p3(20,-10,30); Point3 p1(0,0,1), p2(10,0,2), p3(20,-10,30);
Point3 q1 = expected.transform_from(p1), Point3 q1 = expected.transformFrom(p1),
q2 = expected.transform_from(p2), q2 = expected.transformFrom(p2),
q3 = expected.transform_from(p3); q3 = expected.transformFrom(p3);
Point3Pair ab1(make_pair(q1, p1)); Point3Pair ab1(make_pair(q1, p1));
Point3Pair ab2(make_pair(q2, p2)); Point3Pair ab2(make_pair(q2, p2));
Point3Pair ab3(make_pair(q3, p3)); Point3Pair ab3(make_pair(q3, p3));

6
gtsam/geometry/triangulation.h
View File

@ -256,7 +256,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras // verify that the triangulated point lies in front of all cameras
for(const Pose3& pose: poses) { for(const Pose3& pose: poses) {
const Point3& p_local = pose.transform_to(point); const Point3& p_local = pose.transformTo(point);
if (p_local.z() <= 0) if (p_local.z() <= 0)
throw(TriangulationCheiralityException()); throw(TriangulationCheiralityException());
} }
@ -304,7 +304,7 @@ Point3 triangulatePoint3(
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras // verify that the triangulated point lies in front of all cameras
for(const CAMERA& camera: cameras) { for(const CAMERA& camera: cameras) {
const Point3& p_local = camera.pose().transform_to(point); const Point3& p_local = camera.pose().transformTo(point);
if (p_local.z() <= 0) if (p_local.z() <= 0)
throw(TriangulationCheiralityException()); throw(TriangulationCheiralityException());
} }
@ -484,7 +484,7 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras // verify that the triangulated point lies in front of all cameras
// Only needed if this was not yet handled by exception // Only needed if this was not yet handled by exception
const Point3& p_local = pose.transform_to(point); const Point3& p_local = pose.transformTo(point);
if (p_local.z() <= 0) if (p_local.z() <= 0)
return TriangulationResult::BehindCamera(); return TriangulationResult::BehindCamera();
#endif #endif

2
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -166,7 +166,7 @@ double doubleF(const Pose3& pose, //
} }
Pose3_ x(1); Pose3_ x(1);
Point3_ p(2); Point3_ p(2);
Point3_ p_cam(x, &Pose3::transform_to, p); Point3_ p_cam(x, &Pose3::transformTo, p);
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/nonlinear/utilities.h
View File

@ -241,7 +241,7 @@ Values localToWorld(const Values& local, const Pose2& base,
try { try {
// if value is a Point2, transform it from base pose // if value is a Point2, transform it from base pose
Point2 point = local.at<Point2>(key); Point2 point = local.at<Point2>(key);
world.insert(key, base.transform_from(point)); world.insert(key, base.transformFrom(point));
} catch (std::exception e2) { } catch (std::exception e2) {
// if not Pose2 or Point2, do nothing // if not Pose2 or Point2, do nothing
} }

4
gtsam/slam/ReferenceFrameFactor.h
View File

@ -31,7 +31,7 @@ P transform_point(
const T& trans, const P& global, const T& trans, const P& global,
boost::optional<Matrix&> Dtrans, boost::optional<Matrix&> Dtrans,
boost::optional<Matrix&> Dglobal) { boost::optional<Matrix&> Dglobal) {
return trans.transform_from(global, Dtrans, Dglobal); return trans.transformFrom(global, Dtrans, Dglobal);
} }
/** /**
@ -44,7 +44,7 @@ P transform_point(
* l = lTg * g * l = lTg * g
* *
* The Point and Transform concepts must be Lie types, and the transform * The Point and Transform concepts must be Lie types, and the transform
* relationship "Point = transform_from(Transform, Point)" must exist. * relationship "Point = transformFrom(Transform, Point)" must exist.
* *
* To implement this function in new domains, specialize a new version of * To implement this function in new domains, specialize a new version of
* Point transform_point<Transform,Point>(transform, global, Dtrans, Dglobal) * Point transform_point<Transform,Point>(transform, global, Dtrans, Dglobal)

2
gtsam/slam/dataset.cpp
View File

@ -359,7 +359,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
if (!initial->exists(L(id2))) { if (!initial->exists(L(id2))) {
Pose2 pose = initial->at<Pose2>(id1); Pose2 pose = initial->at<Pose2>(id1);
Point2 local(cos(bearing) * range, sin(bearing) * range); Point2 local(cos(bearing) * range, sin(bearing) * range);
Point2 global = pose.transform_from(local); Point2 global = pose.transformFrom(local);
initial->insert(L(id2), global); initial->insert(L(id2), global);
} }
} }

12
gtsam/slam/expressions.h
View File

@ -21,8 +21,8 @@ typedef Expression<Point2> Point2_;
typedef Expression<Rot2> Rot2_; typedef Expression<Rot2> Rot2_;
typedef Expression<Pose2> Pose2_; typedef Expression<Pose2> Pose2_;
inline Point2_ transform_to(const Pose2_& x, const Point2_& p) { inline Point2_ transformTo(const Pose2_& x, const Point2_& p) {
return Point2_(x, &Pose2::transform_to, p); return Point2_(x, &Pose2::transformTo, p);
} }
// 3D Geometry // 3D Geometry
@ -32,12 +32,12 @@ typedef Expression<Unit3> Unit3_;
typedef Expression<Rot3> Rot3_; typedef Expression<Rot3> Rot3_;
typedef Expression<Pose3> Pose3_; typedef Expression<Pose3> Pose3_;
inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { inline Point3_ transformTo(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transform_to, p); return Point3_(x, &Pose3::transformTo, p);
} }
inline Point3_ transform_from(const Pose3_& x, const Point3_& p) { inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transform_from, p); return Point3_(x, &Pose3::transformFrom, p);
} }
inline Point3_ rotate(const Rot3_& x, const Point3_& p) { inline Point3_ rotate(const Rot3_& x, const Point3_& p) {

2
gtsam/slam/tests/testDataset.cpp
View File

@ -505,7 +505,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
} }
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
Key pointKey = P(j); Key pointKey = P(j);
Point3 point = poseChange.transform_from( readData.tracks[j].p ); Point3 point = poseChange.transformFrom( readData.tracks[j].p );
value.insert(pointKey, point); value.insert(pointKey, point);
} }

2
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -220,7 +220,7 @@ TEST (EssentialMatrixFactor2, factor) {
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
// Check evaluation // Check evaluation
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transformTo(P1);
const Point2 pi = PinholeBase::Project(P2); const Point2 pi = PinholeBase::Project(P2);
Point2 expected(pi - pB(i)); Point2 expected(pi - pB(i));

10
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -89,7 +89,7 @@ TEST( ReferenceFrameFactor, 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 = trans.transform_from(global); Point2 local = trans.transformFrom(global);
PointReferenceFrameFactor tc(lA1, tA1, lB1); PointReferenceFrameFactor tc(lA1, tA1, lB1);
Vector actCost = tc.evaluateError(global, trans, local), Vector actCost = tc.evaluateError(global, trans, local),
@ -124,8 +124,8 @@ TEST( ReferenceFrameFactor, converge_trans ) {
Pose2 transIdeal(7.0, 3.0, M_PI/2); Pose2 transIdeal(7.0, 3.0, M_PI/2);
// verify direction // verify direction
EXPECT(assert_equal(local1, transIdeal.transform_from(global1))); EXPECT(assert_equal(local1, transIdeal.transformFrom(global1)));
EXPECT(assert_equal(local2, transIdeal.transform_from(global2))); EXPECT(assert_equal(local2, transIdeal.transformFrom(global2)));
// choose transform // choose transform
// Pose2 trans = transIdeal; // ideal - works // Pose2 trans = transIdeal; // ideal - works
@ -177,7 +177,7 @@ TEST( ReferenceFrameFactor, 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 = trans.transform_from(global); Point2 idealLocal = trans.transformFrom(global);
// perturb the initial estimate // perturb the initial estimate
// Point2 local = idealLocal; // Ideal case - works // Point2 local = idealLocal; // Ideal case - works
@ -213,7 +213,7 @@ TEST( ReferenceFrameFactor, 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 = trans.inverse().transform_from(local); Point2 idealForeign = trans.inverse().transformFrom(local);
// perturb the initial estimate // perturb the initial estimate
// Point2 global = idealForeign; // Ideal - works // Point2 global = idealForeign; // Ideal - works

2
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -132,7 +132,7 @@ public:
/** /**
* Apply transform to this pose, with optional derivatives * Apply transform to this pose, with optional derivatives
* equivalent to: * equivalent to:
* local = trans.transform_from(global, Dtrans, Dglobal) * local = trans.transformFrom(global, Dtrans, Dglobal)
* *
* Note: the transform jacobian convention is flipped * Note: the transform jacobian convention is flipped
*/ */

4
gtsam_unstable/examples/ConcurrentCalibration.cpp
View File

@ -97,8 +97,8 @@ int main(int argc, char** argv){
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) { if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x)); Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint); initial_estimate.insert(Symbol('l', l), worldPoint);
} }
} }

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -157,7 +157,7 @@ public:
const gtsam::Point2 pn = k_->calibrate(pi); const gtsam::Point2 pn = k_->calibrate(pi);
gtsam::Point3 pc(pn.x(), pn.y(), 1.0); gtsam::Point3 pc(pn.x(), pn.y(), 1.0);
pc = pc/pc.norm(); pc = pc/pc.norm();
gtsam::Point3 pw = pose_.transform_from(pc); gtsam::Point3 pw = pose_.transformFrom(pc);
const gtsam::Point3& pt = pose_.translation(); const gtsam::Point3& pt = pose_.translation();
gtsam::Point3 ray = pw - pt; gtsam::Point3 ray = pw - pt;
double theta = atan2(ray.y(), ray.x()); // longitude double theta = atan2(ray.y(), ray.x()); // longitude

14
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -36,8 +36,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
// normalized points, Aa at origin, Ab at (length, 0.0) // normalized points, Aa at origin, Ab at (length, 0.0)
double len = A.length(); double len = A.length();
if (debug) cout << "len: " << len << endl; if (debug) cout << "len: " << len << endl;
Point2 Ba = transform.transform_to(B.a()), Point2 Ba = transform.transformTo(B.a()),
Bb = transform.transform_to(B.b()); Bb = transform.transformTo(B.b());
if (debug) traits<Point2>::Print(Ba, "Ba"); if (debug) traits<Point2>::Print(Ba, "Ba");
if (debug) traits<Point2>::Print(Bb, "Bb"); if (debug) traits<Point2>::Print(Bb, "Bb");
@ -51,11 +51,11 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
// check conditions for exactly on the same line // check conditions for exactly on the same line
if (Ba.y() == 0.0 && Ba.x() > 0.0 && Ba.x() < len) { if (Ba.y() == 0.0 && Ba.x() > 0.0 && Ba.x() < len) {
if (pt) *pt = transform.transform_from(Ba); if (pt) *pt = transform.transformFrom(Ba);
if (debug) cout << "Ba on the line" << endl; if (debug) cout << "Ba on the line" << endl;
return true; return true;
} else if (Bb.y() == 0.0 && Bb.x() > 0.0 && Bb.x() < len) { } else if (Bb.y() == 0.0 && Bb.x() > 0.0 && Bb.x() < len) {
if (pt) *pt = transform.transform_from(Bb); if (pt) *pt = transform.transformFrom(Bb);
if (debug) cout << "Bb on the line" << endl; if (debug) cout << "Bb on the line" << endl;
return true; return true;
} }
@ -64,7 +64,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
if (fabs(Ba.x() - Bb.x()) < 1e-5) { if (fabs(Ba.x() - Bb.x()) < 1e-5) {
if (debug) cout << "vertical line" << endl; if (debug) cout << "vertical line" << endl;
if (Ba.x() < len && Ba.x() > 0.0) { if (Ba.x() < len && Ba.x() > 0.0) {
if (pt) *pt = transform.transform_from(Point2(Ba.x(), 0.0)); if (pt) *pt = transform.transformFrom(Point2(Ba.x(), 0.0));
if (debug) cout << " within range" << endl; if (debug) cout << " within range" << endl;
return true; return true;
} else { } else {
@ -91,7 +91,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
double xint = (low.x() < high.x()) ? low.x() + fabs(low.y())/slope : high.x() - fabs(high.y())/slope; double xint = (low.x() < high.x()) ? low.x() + fabs(low.y())/slope : high.x() - fabs(high.y())/slope;
if (debug) cout << "xintercept " << xint << endl; if (debug) cout << "xintercept " << xint << endl;
if (xint > 0.0 && xint < len) { if (xint > 0.0 && xint < len) {
if (pt) *pt = transform.transform_from(Point2(xint, 0.0)); if (pt) *pt = transform.transformFrom(Point2(xint, 0.0));
return true; return true;
} else { } else {
if (debug) cout << "xintercept out of range" << endl; if (debug) cout << "xintercept out of range" << endl;
@ -116,7 +116,7 @@ Rot2 SimWall2D::reflection(const Point2& init, const Point2& intersection) const
// translate to put the intersection at the origin and the wall along the x axis // translate to put the intersection at the origin and the wall along the x axis
Rot2 wallAngle = Rot2::relativeBearing(b_ - a_); Rot2 wallAngle = Rot2::relativeBearing(b_ - a_);
Pose2 transform(wallAngle, intersection); Pose2 transform(wallAngle, intersection);
Point2 t_init = transform.transform_to(init); Point2 t_init = transform.transformTo(init);
Point2 t_goal(-t_init.x(), t_init.y()); Point2 t_goal(-t_init.x(), t_init.y());
return Rot2::relativeBearing(wallAngle.rotate(t_goal)); return Rot2::relativeBearing(wallAngle.rotate(t_goal));
} }

4
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -71,7 +71,7 @@ Similarity3 Similarity3::inverse() const {
return Similarity3(Rt, sRt, 1.0 / s_); return Similarity3(Rt, sRt, 1.0 / s_);
} }
Point3 Similarity3::transform_from(const Point3& p, // Point3 Similarity3::transformFrom(const Point3& p, //
OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const {
const Point3 q = R_ * p + t_; const Point3 q = R_ * p + t_;
if (H1) { if (H1) {
@ -86,7 +86,7 @@ Point3 Similarity3::transform_from(const Point3& p, //
} }
Point3 Similarity3::operator*(const Point3& p) const { Point3 Similarity3::operator*(const Point3& p) const {
return transform_from(p); return transformFrom(p);
} }
Matrix4 Similarity3::wedge(const Vector7& xi) { Matrix4 Similarity3::wedge(const Vector7& xi) {

4
gtsam_unstable/geometry/Similarity3.h
View File

@ -96,11 +96,11 @@ public:
/// @{ /// @{
/// Action on a point p is s*(R*p+t) /// Action on a point p is s*(R*p+t)
Point3 transform_from(const Point3& p, // Point3 transformFrom(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = boost::none) const;
/** syntactic sugar for transform_from */ /** syntactic sugar for transformFrom */
Point3 operator*(const Point3& p) const; Point3 operator*(const Point3& p) const;
/// @} /// @}

18
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -232,17 +232,17 @@ TEST(Similarity3, GroupAction) {
Point3 pa = Point3(1, 0, 0); Point3 pa = Point3(1, 0, 0);
Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0); Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0);
Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0); Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0);
EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa))); EXPECT(assert_equal(Point3(2, 2, 3), Ta.transformFrom(pa)));
EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa))); EXPECT(assert_equal(Point3(4, 4, 6), Tb.transformFrom(pa)));
Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0); Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0);
Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0); Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0);
EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa))); EXPECT(assert_equal(Point3(1, 3, 3), Tc.transformFrom(pa)));
EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa))); EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa)));
// Test derivative // Test derivative
boost::function<Point3(Similarity3, Point3)> f = boost::bind( boost::function<Point3(Similarity3, Point3)> f = boost::bind(
&Similarity3::transform_from, _1, _2, boost::none, boost::none); &Similarity3::transformFrom, _1, _2, boost::none, boost::none);
Point3 q(1, 2, 3); Point3 q(1, 2, 3);
for (const auto T : { T1, T2, T3, T4, T5, T6 }) { for (const auto T : { T1, T2, T3, T4, T5, T6 }) {
@ -250,7 +250,7 @@ TEST(Similarity3, GroupAction) {
Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T, q); Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T, q);
Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T, q); Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T, q);
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
T.transform_from(q, actualH1, actualH2); T.transformFrom(q, actualH1, actualH2);
EXPECT(assert_equal(H1, actualH1)); EXPECT(assert_equal(H1, actualH1));
EXPECT(assert_equal(H2, actualH2)); EXPECT(assert_equal(H2, actualH2));
} }
@ -370,9 +370,9 @@ TEST(Similarity3, AlignScaledPointClouds) {
Expression<Point3> q1_(q1), q2_(q2), q3_(q3); Expression<Point3> q1_(q1), q2_(q2), q3_(q3);
// Create prediction expressions // Create prediction expressions
Expression<Point3> predict1(unknownT, &Similarity3::transform_from, q1_); Expression<Point3> predict1(unknownT, &Similarity3::transformFrom, q1_);
Expression<Point3> predict2(unknownT, &Similarity3::transform_from, q2_); Expression<Point3> predict2(unknownT, &Similarity3::transformFrom, q2_);
Expression<Point3> predict3(unknownT, &Similarity3::transform_from, q3_); Expression<Point3> predict3(unknownT, &Similarity3::transformFrom, q3_);
//// Create Expression factor graph //// Create Expression factor graph
// ExpressionFactorGraph graph; // ExpressionFactorGraph graph;

4
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -88,7 +88,7 @@ public:
double theta = landmark(0), phi = landmark(1), rho = landmark(2); double theta = landmark(0), phi = landmark(1), rho = landmark(2);
Point3 pose_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho); Point3 pose_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho);
// Convert the landmark to world coordinates // Convert the landmark to world coordinates
Point3 world_P_landmark = pose.transform_from(pose_P_landmark); Point3 world_P_landmark = pose.transformFrom(pose_P_landmark);
// Project landmark into Pose2 // Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_); PinholeCamera<Cal3_S2> camera(pose, *K_);
return camera.project(world_P_landmark) - measured_; return camera.project(world_P_landmark) - measured_;
@ -208,7 +208,7 @@ public:
double theta = landmark(0), phi = landmark(1), rho = landmark(2); double theta = landmark(0), phi = landmark(1), rho = landmark(2);
Point3 pose1_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho); Point3 pose1_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho);
// Convert the landmark to world coordinates // Convert the landmark to world coordinates
Point3 world_P_landmark = pose1.transform_from(pose1_P_landmark); Point3 world_P_landmark = pose1.transformFrom(pose1_P_landmark);
// Project landmark into Pose2 // Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose2, *K_); PinholeCamera<Cal3_S2> camera(pose2, *K_);
return camera.project(world_P_landmark) - measured_; return camera.project(world_P_landmark) - measured_;

10
gtsam_unstable/slam/TSAMFactors.h
View File

@ -48,7 +48,7 @@ public:
Vector evaluateError(const Pose2& pose, const Point2& point, Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const {
return pose.transform_to(point, H1, H2) - measured_; return pose.transformTo(point, H1, H2) - measured_;
} }
}; };
@ -85,10 +85,10 @@ public:
Matrix D_pose_g_base1, D_pose_g_pose; Matrix D_pose_g_base1, D_pose_g_pose;
Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose); Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose);
Matrix D_point_g_base2, D_point_g_point; Matrix D_point_g_base2, D_point_g_point;
Point2 point_g = base2.transform_from(point, D_point_g_base2, Point2 point_g = base2.transformFrom(point, D_point_g_base2,
D_point_g_point); D_point_g_point);
Matrix D_e_pose_g, D_e_point_g; Matrix D_e_pose_g, D_e_point_g;
Point2 d = pose_g.transform_to(point_g, D_e_pose_g, D_e_point_g); Point2 d = pose_g.transformTo(point_g, D_e_pose_g, D_e_point_g);
if (H1) if (H1)
*H1 = D_e_pose_g * D_pose_g_base1; *H1 = D_e_pose_g * D_pose_g_base1;
if (H2) if (H2)
@ -100,8 +100,8 @@ public:
return d - measured_; return d - measured_;
} else { } else {
Pose2 pose_g = base1.compose(pose); Pose2 pose_g = base1.compose(pose);
Point2 point_g = base2.transform_from(point); Point2 point_g = base2.transformFrom(point);
Point2 d = pose_g.transform_to(point_g); Point2 d = pose_g.transformTo(point_g);
return d - measured_; return d - measured_;
} }
} }

2
gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp
View File

@ -38,7 +38,7 @@ TEST( InvDepthFactorVariant3, optimize) {
Point2 pixel2 = camera2.project(landmark); Point2 pixel2 = camera2.project(landmark);
// Create expected landmark // Create expected landmark
Point3 landmark_p1 = pose1.transform_to(landmark); Point3 landmark_p1 = pose1.transformTo(landmark);
// landmark_p1.print("Landmark in Pose1 Frame:\n"); // landmark_p1.print("Landmark in Pose1 Frame:\n");
double theta = atan2(landmark_p1.x(), landmark_p1.z()); double theta = atan2(landmark_p1.x(), landmark_p1.z());
double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));

10
gtsampy.h
View File

@ -499,8 +499,8 @@ class Pose2 {
static Matrix wedge(double vx, double vy, double w); static Matrix wedge(double vx, double vy, double w);
// Group Actions on Point2 // Group Actions on Point2
gtsam::Point2 transform_from(const gtsam::Point2& p) const; gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
gtsam::Point2 transform_to(const gtsam::Point2& p) const; gtsam::Point2 transformTo(const gtsam::Point2& p) const;
// Standard Interface // Standard Interface
double x() const; double x() const;
@ -546,8 +546,8 @@ class Pose3 {
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
// Group Action on Point3 // Group Action on Point3
gtsam::Point3 transform_from(const gtsam::Point3& p) const; gtsam::Point3 transformFrom(const gtsam::Point3& p) const;
gtsam::Point3 transform_to(const gtsam::Point3& p) const; gtsam::Point3 transformTo(const gtsam::Point3& p) const;
// Standard Interface // Standard Interface
gtsam::Rot3 rotation() const; gtsam::Rot3 rotation() const;
@ -556,7 +556,7 @@ class Pose3 {
double y() const; double y() const;
double z() const; double z() const;
Matrix matrix() const; Matrix matrix() const;
gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() gtsam::Pose3 transformTo(const gtsam::Pose3& pose) const; // FIXME: shadows other transformTo()
double range(const gtsam::Point3& point); double range(const gtsam::Point3& point);
double range(const gtsam::Pose3& pose); double range(const gtsam::Pose3& pose);

2
matlab/+gtsam/cylinderSampleProjection.m
View File

@ -32,7 +32,7 @@ for i = 1:cylinderNum
% Cheirality Exception % Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j}; sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transform_to(sampledPoint3); sampledPoint3local = pose.transformTo(sampledPoint3);
if sampledPoint3local.z <= 0 if sampledPoint3local.z <= 0
continue; continue;
end end

2
matlab/+gtsam/cylinderSampleProjectionStereo.m
View File

@ -24,7 +24,7 @@ for i = 1:cylinderNum
% For Cheirality Exception % For Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j}; sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transform_to(sampledPoint3); sampledPoint3local = pose.transformTo(sampledPoint3);
if sampledPoint3local.z < 0 if sampledPoint3local.z < 0
continue; continue;
end end

2
matlab/gtsam_examples/StereoVOExample_large.m
View File

@ -46,7 +46,7 @@ for i=1:size(measurements,1)
% 3D landmarks are stored in camera coordinates: transform % 3D landmarks are stored in camera coordinates: transform
% to world coordinates using the respective initial pose % to world coordinates using the respective initial pose
pose = initial.atPose3(symbol('x', sf(1))); pose = initial.atPose3(symbol('x', sf(1)));
world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8))); world_point = pose.transformFrom(Point3(sf(6),sf(7),sf(8)));
initial.insert(symbol('l',sf(2)), world_point); initial.insert(symbol('l',sf(2)), world_point);
end end
end end

8
matlab/unstable_examples/testTSAMFactors.m
View File

@ -26,10 +26,10 @@ origin = Pose2;
graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior))
graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior))
graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior))
graph.add(DeltaFactor(10, 1, b1.transform_to(l1), noiseDelta)) graph.add(DeltaFactor(10, 1, b1.transformTo(l1), noiseDelta))
graph.add(DeltaFactor(20, 1, b2.transform_to(l2), noiseDelta)) graph.add(DeltaFactor(20, 1, b2.transformTo(l2), noiseDelta))
graph.add(DeltaFactorBase(100,10, 200,2, b1.transform_to(l2), noiseDelta)) graph.add(DeltaFactorBase(100,10, 200,2, b1.transformTo(l2), noiseDelta))
graph.add(DeltaFactorBase(200,20, 100,1, b2.transform_to(l1), noiseDelta)) graph.add(DeltaFactorBase(200,20, 100,1, b2.transformTo(l1), noiseDelta))
graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom)) graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom))
% Initial values % Initial values

8
python/handwritten/geometry/Pose2.cpp
View File

@ -29,8 +29,8 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose2::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose2::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose2::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose2::compose, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose2::compose, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose2::between, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose2::between, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transform_to, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transformTo, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transform_from, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transformFrom, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose2::bearing, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose2::bearing, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose2::range, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose2::range, 1, 3)
@ -61,9 +61,9 @@ void exportPose2(){
// .def("dim", &Pose2::dim) // .def("dim", &Pose2::dim)
// .def("retract", &Pose2::retract) // .def("retract", &Pose2::retract)
.def("transform_to", &Pose2::transform_to, .def("transformTo", &Pose2::transformTo,
transform_to_overloads(args("point", "H1", "H2"))) transform_to_overloads(args("point", "H1", "H2")))
.def("transform_from", &Pose2::transform_from, .def("transformFrom", &Pose2::transformFrom,
transform_to_overloads(args("point", "H1", "H2"))) transform_to_overloads(args("point", "H1", "H2")))
.def("x", &Pose2::x) .def("x", &Pose2::x)

16
python/handwritten/geometry/Pose3.cpp
View File

@ -30,8 +30,8 @@ using namespace gtsam;
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transformTo, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transformFrom, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(rotation_overloads, Pose3::rotation, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(rotation_overloads, Pose3::rotation, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3)
@ -41,9 +41,9 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3)
void exportPose3(){ void exportPose3(){
// function pointers to desambiguate transform_to() calls // function pointers to desambiguate transformTo() calls
Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transform_to; Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transformTo;
Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transform_to; Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transformTo;
// function pointers to desambiguate compose() calls // function pointers to desambiguate compose() calls
Pose3 (Pose3::*compose1)(const Pose3 &g) const = &Pose3::compose; Pose3 (Pose3::*compose1)(const Pose3 &g) const = &Pose3::compose;
Pose3 (Pose3::*compose2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::compose; Pose3 (Pose3::*compose2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::compose;
@ -69,10 +69,10 @@ void exportPose3(){
.def("identity", &Pose3::identity) .def("identity", &Pose3::identity)
.staticmethod("identity") .staticmethod("identity")
.def("matrix", &Pose3::matrix) .def("matrix", &Pose3::matrix)
.def("transform_from", &Pose3::transform_from, .def("transformFrom", &Pose3::transformFrom,
transform_from_overloads(args("point", "H1", "H2"))) transform_from_overloads(args("point", "H1", "H2")))
.def("transform_to", transform_to1, transform_to_overloads(args("point", "H1", "H2"))) .def("transformTo", transform_to1, transform_to_overloads(args("point", "H1", "H2")))
.def("transform_to", transform_to2) .def("transformTo", transform_to2)
.def("x", &Pose3::x) .def("x", &Pose3::x)
.def("y", &Pose3::y) .def("y", &Pose3::y)
.def("z", &Pose3::z) .def("z", &Pose3::z)

8
tests/testExpressionFactor.cpp
View File

@ -228,7 +228,7 @@ TEST(ExpressionFactor, Shallow) {
Point3_ p_(2); Point3_ p_(2);
// Construct expression, concise evrsion // Construct expression, concise evrsion
Point2_ expression = project(transform_to(x_, p_)); Point2_ expression = project(transformTo(x_, p_));
// Get and check keys and dims // Get and check keys and dims
KeyVector keys; KeyVector keys;
@ -288,7 +288,7 @@ TEST(ExpressionFactor, tree) {
Cal3_S2_ K(3); Cal3_S2_ K(3);
// Create expression tree // Create expression tree
Point3_ p_cam(x, &Pose3::transform_to, p); Point3_ p_cam(x, &Pose3::transformTo, p);
Point2_ xy_hat(Project, p_cam); Point2_ xy_hat(Project, p_cam);
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
@ -301,7 +301,7 @@ TEST(ExpressionFactor, tree) {
// Concise version // Concise version
ExpressionFactor<Point2> f2(model, measured, ExpressionFactor<Point2> f2(model, measured,
uncalibrate(K, project(transform_to(x, p)))); uncalibrate(K, project(transformTo(x, p))));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim()); EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values); boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
@ -461,7 +461,7 @@ TEST(ExpressionFactor, tree_finite_differences) {
Cal3_S2_ K(3); Cal3_S2_ K(3);
// Create expression tree // Create expression tree
Point3_ p_cam(x, &Pose3::transform_to, p); Point3_ p_cam(x, &Pose3::transformTo, p);
Point2_ xy_hat(Project, p_cam); Point2_ xy_hat(Project, p_cam);
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);

4
timing/timeCameraExpression.cpp
View File

@ -68,7 +68,7 @@ int main() {
// 20.3 musecs/call // 20.3 musecs/call
NonlinearFactor::shared_ptr f2 = NonlinearFactor::shared_ptr f2 =
boost::make_shared<ExpressionFactor<Point2> >(model, z, boost::make_shared<ExpressionFactor<Point2> >(model, z,
uncalibrate(K, project(transform_to(x, p)))); uncalibrate(K, project(transformTo(x, p))));
time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values); time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values);
// ExpressionFactor ternary // ExpressionFactor ternary
@ -93,7 +93,7 @@ int main() {
// 16.0 musecs/call // 16.0 musecs/call
NonlinearFactor::shared_ptr g2 = NonlinearFactor::shared_ptr g2 =
boost::make_shared<ExpressionFactor<Point2> >(model, z, boost::make_shared<ExpressionFactor<Point2> >(model, z,
uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); uncalibrate(Cal3_S2_(*fixedK), project(transformTo(x, p))));
time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values); time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values);
// ExpressionFactor, optimized // ExpressionFactor, optimized

2
timing/timeIncremental.cpp
View File

@ -156,7 +156,7 @@ int main(int argc, char *argv[]) {
Rot2 measuredBearing = measurement->measured().bearing(); Rot2 measuredBearing = measurement->measured().bearing();
double measuredRange = measurement->measured().range(); double measuredRange = measurement->measured().range();
newVariables.insert(lmKey, newVariables.insert(lmKey,
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); pose.transformFrom(measuredBearing.rotate(Point2(measuredRange, 0.0))));
} }
} }
else else

2
timing/timeOneCameraExpression.cpp
View File

@ -50,7 +50,7 @@ int main() {
#ifdef TERNARY #ifdef TERNARY
(model, z, project3(x, p, K)); (model, z, project3(x, p, K));
#else #else
(model, z, uncalibrate(K, project(transform_to(x, p)))); (model, z, uncalibrate(K, project(transformTo(x, p))));
#endif #endif
time("timing:", f, values); time("timing:", f, values);

4
timing/timeSFMBALcamTnav.cpp
View File

@ -42,8 +42,8 @@ int main(int argc, char* argv[]) {
Point3_ nav_point_(P(j)); Point3_ nav_point_(P(j));
graph.addExpressionFactor( graph.addExpressionFactor(
gNoiseModel, z, gNoiseModel, z,
uncalibrate(calibration_, // now using transform_from !!!: uncalibrate(calibration_, // now using transformFrom !!!:
project(transform_from(camTnav_, nav_point_)))); project(transformFrom(camTnav_, nav_point_))));
} }
} }

2
timing/timeSFMBALnavTcam.cpp
View File

@ -43,7 +43,7 @@ int main(int argc, char* argv[]) {
graph.addExpressionFactor( graph.addExpressionFactor(
gNoiseModel, z, gNoiseModel, z,
uncalibrate(calibration_, uncalibrate(calibration_,
project(transform_to(navTcam_, nav_point_)))); project(transformTo(navTcam_, nav_point_))));
} }
} }

2
timing/timeSFMExpressions.cpp
View File

@ -61,7 +61,7 @@ int main() {
#ifdef TERNARY #ifdef TERNARY
(model, z, project3(x[i], p[j], K)); (model, z, project3(x[i], p[j], K));
#else #else
(model, z, uncalibrate(K, project(transform_to(x[i], p[j])))); (model, z, uncalibrate(K, project(transformTo(x[i], p[j]))));
#endif #endif
graph.push_back(f); graph.push_back(f);
} }