From 8801de4d635b3185aa45940a320a30d52f63c228 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 16 May 2019 14:33:58 -0400 Subject: [PATCH] Pose3 naming convention --- cython/gtsam/tests/test_Pose3.py | 4 +- examples/SFMExampleExpressions.cpp | 2 +- examples/SolverComparer.cpp | 2 +- examples/StereoVOExample_large.cpp | 4 +- gtsam.h | 11 +- gtsam/geometry/CalibratedCamera.cpp | 6 +- gtsam/geometry/CalibratedCamera.h | 2 +- gtsam/geometry/EssentialMatrix.cpp | 6 +- gtsam/geometry/EssentialMatrix.h | 13 +- gtsam/geometry/PinholePose.h | 2 +- gtsam/geometry/Pose2.cpp | 8 +- gtsam/geometry/Pose2.h | 36 +++- gtsam/geometry/Pose3.cpp | 23 ++- gtsam/geometry/Pose3.h | 42 ++-- gtsam/geometry/StereoCamera.cpp | 8 +- gtsam/geometry/tests/testCalibratedCamera.cpp | 2 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 6 +- gtsam/geometry/tests/testPose2.cpp | 57 +++--- gtsam/geometry/tests/testPose3.cpp | 184 +++++++++--------- gtsam/geometry/triangulation.h | 6 +- gtsam/nonlinear/tests/testExpression.cpp | 2 +- gtsam/nonlinear/utilities.h | 2 +- gtsam/slam/ReferenceFrameFactor.h | 4 +- gtsam/slam/dataset.cpp | 2 +- gtsam/slam/expressions.h | 12 +- gtsam/slam/tests/testDataset.cpp | 2 +- .../slam/tests/testEssentialMatrixFactor.cpp | 2 +- gtsam/slam/tests/testReferenceFrameFactor.cpp | 10 +- gtsam_unstable/dynamics/PoseRTV.h | 2 +- .../examples/ConcurrentCalibration.cpp | 4 +- gtsam_unstable/geometry/InvDepthCamera3.h | 2 +- gtsam_unstable/geometry/SimWall2D.cpp | 14 +- gtsam_unstable/geometry/Similarity3.cpp | 4 +- gtsam_unstable/geometry/Similarity3.h | 4 +- .../geometry/tests/testSimilarity3.cpp | 18 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 4 +- gtsam_unstable/slam/TSAMFactors.h | 10 +- .../slam/tests/testInvDepthFactorVariant3.cpp | 2 +- gtsampy.h | 10 +- matlab/+gtsam/cylinderSampleProjection.m | 2 +- .../+gtsam/cylinderSampleProjectionStereo.m | 2 +- matlab/gtsam_examples/StereoVOExample_large.m | 2 +- matlab/unstable_examples/testTSAMFactors.m | 8 +- python/handwritten/geometry/Pose2.cpp | 8 +- python/handwritten/geometry/Pose3.cpp | 16 +- tests/testExpressionFactor.cpp | 8 +- timing/timeCameraExpression.cpp | 4 +- timing/timeIncremental.cpp | 2 +- timing/timeOneCameraExpression.cpp | 2 +- timing/timeSFMBALcamTnav.cpp | 4 +- timing/timeSFMBALnavTcam.cpp | 2 +- timing/timeSFMExpressions.cpp | 2 +- 52 files changed, 318 insertions(+), 278 deletions(-) diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index 3eb4067c9..577a13304 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -30,9 +30,9 @@ class TestPose3(GtsamTestCase): self.gtsamAssertEquals(actual, expected, 1e-6) def test_transform_to(self): - """Test transform_to method.""" + """Test transformTo method.""" 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) self.gtsamAssertEquals(actual, expected, 1e-6) diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index 30db9144d..191664ef6 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -72,7 +72,7 @@ int main(int argc, char* argv[]) { Point2 measurement = camera.project(points[j]); // Below an expression for the prediction of the measurement: 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 graph.addExpressionFactor(prediction, measurement, measurementNoise); } diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 4300820d1..80ac08e03 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -362,7 +362,7 @@ void runIncremental() Rot2 measuredBearing = measured.bearing(); double measuredRange = measured.range(); newVariables.insert(lmKey, - pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); + pose.transformFrom(measuredBearing.rotate(Point2(measuredRange, 0.0)))); } } else diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 00aaeedb0..5eeb6ac3c 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -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 (!initial_estimate.exists(Symbol('l', l))) { Pose3 camPose = initial_estimate.at(Symbol('x', x)); - //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space - Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); + //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space + Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z)); initial_estimate.insert(Symbol('l', l), worldPoint); } } diff --git a/gtsam.h b/gtsam.h index 220a2c99f..0893cf661 100644 --- a/gtsam.h +++ b/gtsam.h @@ -632,8 +632,8 @@ class Pose2 { static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 - gtsam::Point2 transform_from(const gtsam::Point2& p) const; - gtsam::Point2 transform_to(const gtsam::Point2& p) const; + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Point2 transformTo(const gtsam::Point2& p) const; // Standard Interface double x() const; @@ -685,8 +685,8 @@ class Pose3 { static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& point) const; - gtsam::Point3 transform_to(const gtsam::Point3& point) const; + gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point) const; // Standard Interface gtsam::Rot3 rotation() const; @@ -695,7 +695,8 @@ class Pose3 { double y() const; double z() 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::Pose3& pose); diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 3c9fb1563..dd88f9f69 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -107,7 +107,7 @@ Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { /* ************************************************************************* */ pair PinholeBase::projectSafe(const Point3& pw) const { - const Point3 pc = pose().transform_to(pw); + const Point3 pc = pose().transformTo(pw); const Point2 pn = Project(pc); return make_pair(pn, pc.z() > 0); } @@ -116,8 +116,8 @@ pair PinholeBase::projectSafe(const Point3& pw) const { Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { - Matrix3 Rt; // calculated by transform_to if needed - const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); + Matrix3 Rt; // calculated by transformTo if needed + const Point3 q = pose().transformTo(point, boost::none, Dpoint ? &Rt : 0); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (q.z() <= 0) throw CheiralityException(); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index acb3cacaf..feb004b71 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -351,7 +351,7 @@ public: Dresult_ddepth ? &Dpoint_ddepth : 0); Matrix33 Dresult_dpoint; - const Point3 result = pose().transform_from(point, Dresult_dpose, + const Point3 result = pose().transformFrom(point, Dresult_dpose, (Dresult_ddepth || Dresult_dp) ? &Dresult_dpoint : 0); diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 02ede9228..020c94fdb 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -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 { Pose3 pose(rotation(), direction().point3()); Matrix36 DE_; - Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); + Point3 q = pose.transformTo(p, DE ? &DE_ : 0, Dpoint); 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 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 diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 308250033..891902da7 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -138,7 +138,7 @@ class GTSAM_EXPORT EssentialMatrix { * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in pose coordinates */ - Point3 transform_to(const Point3& p, + Point3 transformTo(const Point3& p, OptionalJacobian<3, 5> DE = boost::none, 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: /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index afc1c19ef..935962423 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -145,7 +145,7 @@ public: (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0, Dresult_ddepth ? &Dpoint_ddepth : 0); Matrix33 Dresult_dpoint; - const Point3 result = pose().transform_from(point, Dresult_dpose, + const Point3 result = pose().transformFrom(point, Dresult_dpose, (Dresult_ddepth || Dresult_dp || Dresult_dcal) ? &Dresult_dpoint : 0); diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 2a52e98ba..4e5085cfe 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -198,7 +198,7 @@ Pose2 Pose2::inverse() const { /* ************************************************************************* */ // 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, 2> Htranslation = Hpose.cols<2>(0); 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 -Point2 Pose2::transform_from(const Point2& point, +Point2 Pose2::transformFrom(const Point2& point, OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const { OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); 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 { // make temporary matrices 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); Matrix12 D_result_d; 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 * 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 * using dp=[dpx;dpy] and q=[dqx;dqy] we have * |dqx| |c -s| |dpx| |dpx -dpy| |c| diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 6937ff78d..efd6a7f88 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -195,17 +195,17 @@ public: /// @{ /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point, - OptionalJacobian<2, 3> H1 = boost::none, - OptionalJacobian<2, 2> H2 = boost::none) const; + Point2 transformTo(const Point2& point, + OptionalJacobian<2, 3> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const; /** Return point coordinates in global frame */ - Point2 transform_from(const Point2& point, - OptionalJacobian<2, 3> H1 = boost::none, - OptionalJacobian<2, 2> H2 = boost::none) const; + Point2 transformFrom(const Point2& point, + OptionalJacobian<2, 3> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const; - /** syntactic sugar for transform_from */ - inline Point2 operator*(const Point2& point) const { return transform_from(point);} + /** syntactic sugar for transformFrom */ + inline Point2 operator*(const Point2& point) const { return transformFrom(point);} /// @} /// @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 friend class boost::serialization::access; @@ -313,7 +329,7 @@ inline Matrix wedge(const Vector& xi) { /** * 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 Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 334660ca8..e720fe0b9 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -284,6 +284,12 @@ Matrix4 Pose3::matrix() const { return mat; } +/* ************************************************************************* */ +Pose3 Pose3::transformPoseFrom(const Pose3& aTb) const { + const Pose3& wTa = *this; + return wTa * aTb; +} + /* ************************************************************************* */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 Pose3 Pose3::transform_to(const Pose3& pose) const { @@ -294,15 +300,16 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { #endif /* ************************************************************************* */ -Pose3 Pose3::transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1, - OptionalJacobian<6, 6> H2) const { - if (H1) *H1 = -pose.inverse().AdjointMap() * AdjointMap(); +Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> H1, + OptionalJacobian<6, 6> H2) const { + if (H1) *H1 = -wTb.inverse().AdjointMap() * AdjointMap(); 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 { // Only get matrix once, to avoid multiple allocations, // 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 { // Only get transpose once, to avoid multiple allocations, // 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 { Matrix36 D_local_pose; 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) { return local.norm(); } else { @@ -368,7 +375,7 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, OptionalJacobian<2, 3> H2) const { Matrix36 D_local_pose; 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) { return Unit3(local); } else { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 0a27e5f17..e90f91127 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -81,7 +81,6 @@ public: /** * 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 - * 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. */ static boost::optional Align(const std::vector& abPointPairs); @@ -207,12 +206,12 @@ public: * @param Dpoint optional 3*3 Jacobian wrpt point * @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; - /** syntactic sugar for transform_from */ + /** syntactic sugar for transformFrom */ 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 * @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; /// @} @@ -253,9 +252,12 @@ public: /** convert to 4*4 matrix */ 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 */ - Pose3 transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, - OptionalJacobian<6, 6> H2 = boost::none) const; + Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; /** * Calculate range to a landmark @@ -319,7 +321,23 @@ public: #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @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; /// @} #endif @@ -354,12 +372,10 @@ inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); } -/** - * Calculate pose between a vector of 3D point correspondences (b_point, a_point) - * where a_point = Pose3::transform_from(b_point) = t + R*b_point - * @deprecated: use Pose3::Align with point pairs ordered the opposite way - */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +// deprecated: use Pose3::Align with point pairs ordered the opposite way GTSAM_EXPORT boost::optional align(const std::vector& baPointPairs); +#endif // For MATLAB wrapper typedef std::vector Pose3Vector; diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index c34d4d8f3..54cdee210 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -37,7 +37,7 @@ namespace gtsam { StereoPoint2 StereoCamera::project2(const Point3& point, 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 if (q.z() <= 0) @@ -94,7 +94,7 @@ namespace gtsam { double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); double X = Z * (measured[0] - K_->px()) / K_->fx(); 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; } @@ -120,7 +120,7 @@ namespace gtsam { -z_partial_uR, z_partial_uR, 0; 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) { *H2 = D_point_local * D_local_z; @@ -129,7 +129,7 @@ namespace gtsam { return point; } - return leftCamPose_.transform_from(local); + return leftCamPose_.transformFrom(local); } } diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 33a88db1f..f5f824d05 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -134,7 +134,7 @@ TEST( CalibratedCamera, Dproject_point_pose) Point2 result = camera.project(point1, Dpose, Dpoint); Matrix numerical_pose = numericalDerivative21(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(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 75ee9427d..7ba885115 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -115,9 +115,9 @@ TEST (EssentialMatrix, RoundTrip) { //************************************************************************* 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 Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4) * Rot3::Roll(M_PI / 6.0); @@ -126,7 +126,7 @@ TEST (EssentialMatrix, transform_to) { //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0))); static Point3 P(0.2, 0.7, -2); Matrix actH1, actH2; - E.transform_to(P, actH1, actH2); + E.transformTo(P, actH1, actH2); Matrix expH1 = numericalDerivative21(transform_to_, E, P), // expH2 = numericalDerivative22(transform_to_, E, P); EXPECT(assert_equal(expH1, actH1, 1e-8)); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 20631bdd6..b8b3fc52c 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -266,45 +266,44 @@ TEST( Pose2, LogmapDerivative2) { } /* ************************************************************************* */ -static Point2 transform_to_proxy(const Pose2& pose, const Point2& point) { - return pose.transform_to(point); +static Point2 transformTo_(const Pose2& pose, const Point2& point) { + return pose.transformTo(point); } -TEST( Pose2, transform_to ) -{ - Pose2 pose(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y - Point2 point(-1,4); // landmark at (-1,4) +TEST(Pose2, transformTo) { + Pose2 pose(M_PI / 2.0, Point2(1, 2)); // robot at (1,2) looking towards y + Point2 point(-1, 4); // landmark at (-1,4) // expected - Point2 expected(2,2); - Matrix expectedH1 = (Matrix(2,3) << -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(); + Point2 expected(2, 2); + Matrix expectedH1 = + (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 Matrix actualH1, actualH2; - Point2 actual = pose.transform_to(point, actualH1, actualH2); - EXPECT(assert_equal(expected,actual)); + Point2 actual = pose.transformTo(point, actualH1, actualH2); + EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(expectedH1,actualH1)); - Matrix numericalH1 = numericalDerivative21(transform_to_proxy, pose, point); - EXPECT(assert_equal(numericalH1,actualH1)); + EXPECT(assert_equal(expectedH1, actualH1)); + Matrix numericalH1 = numericalDerivative21(transformTo_, pose, point); + EXPECT(assert_equal(numericalH1, actualH1)); - EXPECT(assert_equal(expectedH2,actualH2)); - Matrix numericalH2 = numericalDerivative22(transform_to_proxy, pose, point); - EXPECT(assert_equal(numericalH2,actualH2)); + EXPECT(assert_equal(expectedH2, actualH2)); + Matrix numericalH2 = numericalDerivative22(transformTo_, pose, point); + EXPECT(assert_equal(numericalH2, actualH2)); } /* ************************************************************************* */ -static Point2 transform_from_proxy(const Pose2& pose, const Point2& point) { - return pose.transform_from(point); +static Point2 transformFrom_(const Pose2& pose, const Point2& point) { + return pose.transformFrom(point); } -TEST (Pose2, transform_from) -{ - Pose2 pose(1., 0., M_PI/2.0); +TEST(Pose2, transformFrom) { + Pose2 pose(1., 0., M_PI / 2.0); Point2 pt(2., 1.); Matrix H1, H2; - Point2 actual = pose.transform_from(pt, H1, H2); + Point2 actual = pose.transformFrom(pt, H1, H2); Point2 expected(0., 2.); 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 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, 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, numericalH2)); } @@ -349,8 +348,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 = (pose1 * pose2).transform_to(point); - Point2 actual_point2 = pose2.transform_to(pose1.transform_to(point)); + Point2 actual_point1 = (pose1 * pose2).transformTo(point); + Point2 actual_point2 = pose2.transformTo(pose1.transformTo(point)); EXPECT(assert_equal(expected_point, actual_point1)); EXPECT(assert_equal(expected_point, actual_point2)); } @@ -735,7 +734,7 @@ TEST(Pose2, align_2) { vector correspondences; 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,20),q2)); Point2Pair pq1(make_pair(p1, q1)); @@ -750,7 +749,7 @@ namespace align_3 { Point2 t(10,10); Pose2 expected(Rot2::fromAngle(2*M_PI/3), t); 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) { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9a6cce753..0b2f59773 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -316,172 +316,162 @@ TEST( Pose3, compose_inverse) } /* ************************************************************************* */ -Point3 transform_from_(const Pose3& pose, const Point3& point) { return pose.transform_from(point); } -TEST( Pose3, Dtransform_from1_a) -{ +Point3 transformFrom_(const Pose3& pose, const Point3& point) { + return pose.transformFrom(point); +} +TEST(Pose3, Dtransform_from1_a) { Matrix actualDtransform_from1; - T.transform_from(P, actualDtransform_from1, boost::none); - Matrix numerical = numericalDerivative21(transform_from_,T,P); - EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); + T.transformFrom(P, actualDtransform_from1, boost::none); + Matrix numerical = numericalDerivative21(transformFrom_, T, P); + EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } -TEST( Pose3, Dtransform_from1_b) -{ +TEST(Pose3, Dtransform_from1_b) { Pose3 origin; Matrix actualDtransform_from1; - origin.transform_from(P, actualDtransform_from1, boost::none); - Matrix numerical = numericalDerivative21(transform_from_,origin,P); - EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); + origin.transformFrom(P, actualDtransform_from1, boost::none); + Matrix numerical = numericalDerivative21(transformFrom_, origin, P); + EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } -TEST( Pose3, Dtransform_from1_c) -{ - Point3 origin(0,0,0); - Pose3 T0(R,origin); +TEST(Pose3, Dtransform_from1_c) { + Point3 origin(0, 0, 0); + Pose3 T0(R, origin); Matrix actualDtransform_from1; - T0.transform_from(P, actualDtransform_from1, boost::none); - Matrix numerical = numericalDerivative21(transform_from_,T0,P); - EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); + T0.transformFrom(P, actualDtransform_from1, boost::none); + Matrix numerical = numericalDerivative21(transformFrom_, T0, P); + EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } -TEST( Pose3, Dtransform_from1_d) -{ +TEST(Pose3, Dtransform_from1_d) { Rot3 I; - Point3 t0(100,0,0); - Pose3 T0(I,t0); + Point3 t0(100, 0, 0); + Pose3 T0(I, t0); Matrix actualDtransform_from1; - T0.transform_from(P, actualDtransform_from1, boost::none); - //print(computed, "Dtransform_from1_d computed:"); - Matrix numerical = numericalDerivative21(transform_from_,T0,P); - //print(numerical, "Dtransform_from1_d numerical:"); - EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); + T0.transformFrom(P, actualDtransform_from1, boost::none); + // print(computed, "Dtransform_from1_d computed:"); + Matrix numerical = numericalDerivative21(transformFrom_, T0, P); + // print(numerical, "Dtransform_from1_d numerical:"); + EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, Dtransform_from2) -{ +TEST(Pose3, Dtransform_from2) { Matrix actualDtransform_from2; - T.transform_from(P, boost::none, actualDtransform_from2); - Matrix numerical = numericalDerivative22(transform_from_,T,P); - EXPECT(assert_equal(numerical,actualDtransform_from2,1e-8)); + T.transformFrom(P, boost::none, actualDtransform_from2); + Matrix numerical = numericalDerivative22(transformFrom_, T, P); + EXPECT(assert_equal(numerical, actualDtransform_from2, 1e-8)); } /* ************************************************************************* */ -Point3 transform_to_(const Pose3& pose, const Point3& point) { return pose.transform_to(point); } -TEST( Pose3, Dtransform_to1) -{ +Point3 transform_to_(const Pose3& pose, const Point3& point) { + return pose.transformTo(point); +} +TEST(Pose3, Dtransform_to1) { Matrix computed; - T.transform_to(P, computed, boost::none); - Matrix numerical = numericalDerivative21(transform_to_,T,P); - EXPECT(assert_equal(numerical,computed,1e-8)); + T.transformTo(P, computed, boost::none); + Matrix numerical = numericalDerivative21(transform_to_, T, P); + EXPECT(assert_equal(numerical, computed, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, Dtransform_to2) -{ +TEST(Pose3, Dtransform_to2) { Matrix computed; - T.transform_to(P, boost::none, computed); - Matrix numerical = numericalDerivative22(transform_to_,T,P); - EXPECT(assert_equal(numerical,computed,1e-8)); + T.transformTo(P, boost::none, computed); + Matrix numerical = numericalDerivative22(transform_to_, T, P); + EXPECT(assert_equal(numerical, computed, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_to_with_derivatives) -{ +TEST(Pose3, transform_to_with_derivatives) { Matrix actH1, actH2; - T.transform_to(P,actH1,actH2); - Matrix expH1 = numericalDerivative21(transform_to_, T,P), - expH2 = numericalDerivative22(transform_to_, T,P); + T.transformTo(P, actH1, actH2); + Matrix expH1 = numericalDerivative21(transform_to_, T, P), + expH2 = numericalDerivative22(transform_to_, T, P); EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_from_with_derivatives) -{ +TEST(Pose3, transform_from_with_derivatives) { Matrix actH1, actH2; - T.transform_from(P,actH1,actH2); - Matrix expH1 = numericalDerivative21(transform_from_, T,P), - expH2 = numericalDerivative22(transform_from_, T,P); + T.transformFrom(P, actH1, actH2); + Matrix expH1 = numericalDerivative21(transformFrom_, T, P), + expH2 = numericalDerivative22(transformFrom_, T, P); EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_to_translate) -{ - Point3 actual = Pose3(Rot3(), Point3(1, 2, 3)).transform_to(Point3(10.,20.,30.)); - Point3 expected(9.,18.,27.); - EXPECT(assert_equal(expected, actual)); +TEST(Pose3, transform_to_translate) { + Point3 actual = + Pose3(Rot3(), Point3(1, 2, 3)).transformTo(Point3(10., 20., 30.)); + Point3 expected(9., 18., 27.); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST( Pose3, transform_to_rotate) -{ - Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0)); - Point3 actual = transform.transform_to(Point3(2,1,10)); - Point3 expected(-1,2,10); - EXPECT(assert_equal(expected, actual, 0.001)); +TEST(Pose3, transform_to_rotate) { + Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(0, 0, 0)); + Point3 actual = transform.transformTo(Point3(2, 1, 10)); + Point3 expected(-1, 2, 10); + EXPECT(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ -TEST( Pose3, transform_to) -{ - Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(2,4, 0)); - Point3 actual = transform.transform_to(Point3(3,2,10)); - Point3 expected(2,1,10); - EXPECT(assert_equal(expected, actual, 0.001)); +TEST(Pose3, transformTo) { + Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)); + Point3 actual = transform.transformTo(Point3(3, 2, 10)); + Point3 expected(2, 1, 10); + 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) -{ - Pose3 origin = T.transform_pose_to(T); +TEST(Pose3, transformPoseTo) { + Pose3 origin = T.transformPoseTo(T); EXPECT(assert_equal(Pose3{}, origin)); } /* ************************************************************************* */ -TEST( Pose3, transform_pose_to_with_derivatives) -{ +TEST(Pose3, transformPoseTo_with_derivatives) { 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))); - Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T2), - expH2 = numericalDerivative22(transform_pose_to_, T, T2); + Matrix expH1 = numericalDerivative21(transformPoseTo_, T, T2), + expH2 = numericalDerivative22(transformPoseTo_, T, T2); EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_pose_to_with_derivatives2) -{ +TEST(Pose3, transformPoseTo_with_derivatives2) { 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))); - Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T3), - expH2 = numericalDerivative22(transform_pose_to_, T, T3); + Matrix expH1 = numericalDerivative21(transformPoseTo_, T, T3), + expH2 = numericalDerivative22(transformPoseTo_, T, T3); EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_from) -{ - Point3 actual = T3.transform_from(Point3(0,0,0)); - Point3 expected = Point3(1.,2.,3.); - EXPECT(assert_equal(expected, actual)); +TEST(Pose3, transformFrom) { + Point3 actual = T3.transformFrom(Point3(0, 0, 0)); + Point3 expected = Point3(1., 2., 3.); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST( Pose3, transform_roundtrip) -{ - Point3 actual = T3.transform_from(T3.transform_to(Point3(12., -0.11,7.0))); - Point3 expected(12., -0.11,7.0); - EXPECT(assert_equal(expected, actual)); +TEST(Pose3, transform_roundtrip) { + Point3 actual = T3.transformFrom(T3.transformTo(Point3(12., -0.11, 7.0))); + Point3 expected(12., -0.11, 7.0); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ @@ -737,9 +727,9 @@ TEST(Pose3, Align2) { vector correspondences; Point3 p1(0,0,1), p2(10,0,2), p3(20,-10,30); - Point3 q1 = expected.transform_from(p1), - q2 = expected.transform_from(p2), - q3 = expected.transform_from(p3); + Point3 q1 = expected.transformFrom(p1), + q2 = expected.transformFrom(p2), + q3 = expected.transformFrom(p3); Point3Pair ab1(make_pair(q1, p1)); Point3Pair ab2(make_pair(q2, p2)); Point3Pair ab3(make_pair(q3, p3)); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index b2d8c3be9..851c5e4d3 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -256,7 +256,7 @@ Point3 triangulatePoint3(const std::vector& poses, #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras 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) throw(TriangulationCheiralityException()); } @@ -304,7 +304,7 @@ Point3 triangulatePoint3( #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all 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) throw(TriangulationCheiralityException()); } @@ -484,7 +484,7 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras // 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) return TriangulationResult::BehindCamera(); #endif diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 7a06179b4..67e0d1f24 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -166,7 +166,7 @@ double doubleF(const Pose3& pose, // } Pose3_ x(1); Point3_ p(2); -Point3_ p_cam(x, &Pose3::transform_to, p); +Point3_ p_cam(x, &Pose3::transformTo, p); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 58e7e78af..867db70e0 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -241,7 +241,7 @@ Values localToWorld(const Values& local, const Pose2& base, try { // if value is a Point2, transform it from base pose Point2 point = local.at(key); - world.insert(key, base.transform_from(point)); + world.insert(key, base.transformFrom(point)); } catch (std::exception e2) { // if not Pose2 or Point2, do nothing } diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index a1a267116..30f761934 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -31,7 +31,7 @@ P transform_point( const T& trans, const P& global, boost::optional Dtrans, boost::optional Dglobal) { - return trans.transform_from(global, Dtrans, Dglobal); + return trans.transformFrom(global, Dtrans, Dglobal); } /** @@ -44,7 +44,7 @@ P transform_point( * l = lTg * g * * 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 * Point transform_point(transform, global, Dtrans, Dglobal) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b66f22ced..8a5c87ecf 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -359,7 +359,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (!initial->exists(L(id2))) { Pose2 pose = initial->at(id1); Point2 local(cos(bearing) * range, sin(bearing) * range); - Point2 global = pose.transform_from(local); + Point2 global = pose.transformFrom(local); initial->insert(L(id2), global); } } diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index f9f7125cc..fc2f60f35 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -21,8 +21,8 @@ typedef Expression Point2_; typedef Expression Rot2_; typedef Expression Pose2_; -inline Point2_ transform_to(const Pose2_& x, const Point2_& p) { - return Point2_(x, &Pose2::transform_to, p); +inline Point2_ transformTo(const Pose2_& x, const Point2_& p) { + return Point2_(x, &Pose2::transformTo, p); } // 3D Geometry @@ -32,12 +32,12 @@ typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; -inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { - return Point3_(x, &Pose3::transform_to, p); +inline Point3_ transformTo(const Pose3_& x, const Point3_& p) { + return Point3_(x, &Pose3::transformTo, p); } -inline Point3_ transform_from(const Pose3_& x, const Point3_& p) { - return Point3_(x, &Pose3::transform_from, p); +inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) { + return Point3_(x, &Pose3::transformFrom, p); } inline Point3_ rotate(const Rot3_& x, const Point3_& p) { diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 9434280d4..08de83eb1 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -505,7 +505,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ } for(size_t j=0; j < readData.number_tracks(); j++){ // for each point 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); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index d33551edf..b2d368b67 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -220,7 +220,7 @@ TEST (EssentialMatrixFactor2, factor) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); // 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); Point2 expected(pi - pB(i)); diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 9cb55d82f..217ff2178 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -89,7 +89,7 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { // get values that are ideal Pose2 trans(2.0, 3.0, 0.0); Point2 global(5.0, 6.0); - Point2 local = trans.transform_from(global); + Point2 local = trans.transformFrom(global); PointReferenceFrameFactor tc(lA1, tA1, lB1); Vector actCost = tc.evaluateError(global, trans, local), @@ -124,8 +124,8 @@ TEST( ReferenceFrameFactor, converge_trans ) { Pose2 transIdeal(7.0, 3.0, M_PI/2); // verify direction - EXPECT(assert_equal(local1, transIdeal.transform_from(global1))); - EXPECT(assert_equal(local2, transIdeal.transform_from(global2))); + EXPECT(assert_equal(local1, transIdeal.transformFrom(global1))); + EXPECT(assert_equal(local2, transIdeal.transformFrom(global2))); // choose transform // 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, 3.1); // significant rotation - Point2 idealLocal = trans.transform_from(global); + Point2 idealLocal = trans.transformFrom(global); // perturb the initial estimate // 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, 3.1); // significant rotation - Point2 idealForeign = trans.inverse().transform_from(local); + Point2 idealForeign = trans.inverse().transformFrom(local); // perturb the initial estimate // Point2 global = idealForeign; // Ideal - works diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index bf92ab9c4..0c00e5d95 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -132,7 +132,7 @@ public: /** * Apply transform to this pose, with optional derivatives * equivalent to: - * local = trans.transform_from(global, Dtrans, Dglobal) + * local = trans.transformFrom(global, Dtrans, Dglobal) * * Note: the transform jacobian convention is flipped */ diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index c930a1505..41e75432f 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -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 (!initial_estimate.exists(Symbol('l', l))) { Pose3 camPose = initial_estimate.at(Symbol('x', x)); - //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space - Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); + //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space + Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z)); initial_estimate.insert(Symbol('l', l), worldPoint); } } diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 4f1b42610..70ec6f513 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -157,7 +157,7 @@ public: const gtsam::Point2 pn = k_->calibrate(pi); gtsam::Point3 pc(pn.x(), pn.y(), 1.0); pc = pc/pc.norm(); - gtsam::Point3 pw = pose_.transform_from(pc); + gtsam::Point3 pw = pose_.transformFrom(pc); const gtsam::Point3& pt = pose_.translation(); gtsam::Point3 ray = pw - pt; double theta = atan2(ray.y(), ray.x()); // longitude diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index 111d23b91..99545a5be 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -36,8 +36,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons // normalized points, Aa at origin, Ab at (length, 0.0) double len = A.length(); if (debug) cout << "len: " << len << endl; - Point2 Ba = transform.transform_to(B.a()), - Bb = transform.transform_to(B.b()); + Point2 Ba = transform.transformTo(B.a()), + Bb = transform.transformTo(B.b()); if (debug) traits::Print(Ba, "Ba"); if (debug) traits::Print(Bb, "Bb"); @@ -51,11 +51,11 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons // check conditions for exactly on the same line 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; return true; } 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; return true; } @@ -64,7 +64,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons if (fabs(Ba.x() - Bb.x()) < 1e-5) { if (debug) cout << "vertical line" << endl; 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; return true; } else { @@ -91,7 +91,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons 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 (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; } else { 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 Rot2 wallAngle = Rot2::relativeBearing(b_ - a_); 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()); return Rot2::relativeBearing(wallAngle.rotate(t_goal)); } diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 9768f4fa4..d40fb0b59 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -71,7 +71,7 @@ Similarity3 Similarity3::inverse() const { 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 { const Point3 q = R_ * p + t_; if (H1) { @@ -86,7 +86,7 @@ Point3 Similarity3::transform_from(const Point3& p, // } Point3 Similarity3::operator*(const Point3& p) const { - return transform_from(p); + return transformFrom(p); } Matrix4 Similarity3::wedge(const Vector7& xi) { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 853261fc2..f7ba53d87 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -96,11 +96,11 @@ public: /// @{ /// 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, 3> H2 = boost::none) const; - /** syntactic sugar for transform_from */ + /** syntactic sugar for transformFrom */ Point3 operator*(const Point3& p) const; /// @} diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 03b772a4f..aaeb0854d 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -232,17 +232,17 @@ TEST(Similarity3, GroupAction) { Point3 pa = Point3(1, 0, 0); Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.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(4, 4, 6), Tb.transform_from(pa))); + EXPECT(assert_equal(Point3(2, 2, 3), Ta.transformFrom(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 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(2, 6, 6), Td.transform_from(pa))); + EXPECT(assert_equal(Point3(1, 3, 3), Tc.transformFrom(pa))); + EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa))); // Test derivative boost::function 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); for (const auto T : { T1, T2, T3, T4, T5, T6 }) { @@ -250,7 +250,7 @@ TEST(Similarity3, GroupAction) { Matrix H1 = numericalDerivative21(f, T, q); Matrix H2 = numericalDerivative22(f, T, q); Matrix actualH1, actualH2; - T.transform_from(q, actualH1, actualH2); + T.transformFrom(q, actualH1, actualH2); EXPECT(assert_equal(H1, actualH1)); EXPECT(assert_equal(H2, actualH2)); } @@ -370,9 +370,9 @@ TEST(Similarity3, AlignScaledPointClouds) { Expression q1_(q1), q2_(q2), q3_(q3); // Create prediction expressions - Expression predict1(unknownT, &Similarity3::transform_from, q1_); - Expression predict2(unknownT, &Similarity3::transform_from, q2_); - Expression predict3(unknownT, &Similarity3::transform_from, q3_); + Expression predict1(unknownT, &Similarity3::transformFrom, q1_); + Expression predict2(unknownT, &Similarity3::transformFrom, q2_); + Expression predict3(unknownT, &Similarity3::transformFrom, q3_); //// Create Expression factor graph // ExpressionFactorGraph graph; diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 3041f5f23..83df9e13b 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -88,7 +88,7 @@ public: 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); // 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 PinholeCamera camera(pose, *K_); return camera.project(world_P_landmark) - measured_; @@ -208,7 +208,7 @@ public: 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); // 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 PinholeCamera camera(pose2, *K_); return camera.project(world_P_landmark) - measured_; diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index d59e33724..0a456e59c 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -48,7 +48,7 @@ public: Vector evaluateError(const Pose2& pose, const Point2& point, boost::optional H1 = boost::none, boost::optional H2 = 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; Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose); 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); 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) *H1 = D_e_pose_g * D_pose_g_base1; if (H2) @@ -100,8 +100,8 @@ public: return d - measured_; } else { Pose2 pose_g = base1.compose(pose); - Point2 point_g = base2.transform_from(point); - Point2 d = pose_g.transform_to(point_g); + Point2 point_g = base2.transformFrom(point); + Point2 d = pose_g.transformTo(point_g); return d - measured_; } } diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index 976a3f43d..951349b0f 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -38,7 +38,7 @@ TEST( InvDepthFactorVariant3, optimize) { Point2 pixel2 = camera2.project(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"); 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())); diff --git a/gtsampy.h b/gtsampy.h index 141fca79d..bbfd296eb 100644 --- a/gtsampy.h +++ b/gtsampy.h @@ -499,8 +499,8 @@ class Pose2 { static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 - gtsam::Point2 transform_from(const gtsam::Point2& p) const; - gtsam::Point2 transform_to(const gtsam::Point2& p) const; + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Point2 transformTo(const gtsam::Point2& p) const; // Standard Interface double x() const; @@ -546,8 +546,8 @@ class Pose3 { static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& p) const; - gtsam::Point3 transform_to(const gtsam::Point3& p) const; + gtsam::Point3 transformFrom(const gtsam::Point3& p) const; + gtsam::Point3 transformTo(const gtsam::Point3& p) const; // Standard Interface gtsam::Rot3 rotation() const; @@ -556,7 +556,7 @@ class Pose3 { double y() const; double z() 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::Pose3& pose); diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 0abd9cc3c..71f72f6b9 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -32,7 +32,7 @@ for i = 1:cylinderNum % Cheirality Exception sampledPoint3 = cylinders{i}.Points{j}; - sampledPoint3local = pose.transform_to(sampledPoint3); + sampledPoint3local = pose.transformTo(sampledPoint3); if sampledPoint3local.z <= 0 continue; end diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 6512231e8..10409ac6d 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -24,7 +24,7 @@ for i = 1:cylinderNum % For Cheirality Exception sampledPoint3 = cylinders{i}.Points{j}; - sampledPoint3local = pose.transform_to(sampledPoint3); + sampledPoint3local = pose.transformTo(sampledPoint3); if sampledPoint3local.z < 0 continue; end diff --git a/matlab/gtsam_examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m index 464448335..fff0ae2d7 100644 --- a/matlab/gtsam_examples/StereoVOExample_large.m +++ b/matlab/gtsam_examples/StereoVOExample_large.m @@ -46,7 +46,7 @@ for i=1:size(measurements,1) % 3D landmarks are stored in camera coordinates: transform % to world coordinates using the respective initial pose 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); end end diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m index 341725723..5cfd0aa80 100644 --- a/matlab/unstable_examples/testTSAMFactors.m +++ b/matlab/unstable_examples/testTSAMFactors.m @@ -26,10 +26,10 @@ origin = Pose2; graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) -graph.add(DeltaFactor(10, 1, b1.transform_to(l1), noiseDelta)) -graph.add(DeltaFactor(20, 1, b2.transform_to(l2), noiseDelta)) -graph.add(DeltaFactorBase(100,10, 200,2, b1.transform_to(l2), noiseDelta)) -graph.add(DeltaFactorBase(200,20, 100,1, b2.transform_to(l1), noiseDelta)) +graph.add(DeltaFactor(10, 1, b1.transformTo(l1), noiseDelta)) +graph.add(DeltaFactor(20, 1, b2.transformTo(l2), noiseDelta)) +graph.add(DeltaFactorBase(100,10, 200,2, b1.transformTo(l2), noiseDelta)) +graph.add(DeltaFactorBase(200,20, 100,1, b2.transformTo(l1), noiseDelta)) graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom)) % Initial values diff --git a/python/handwritten/geometry/Pose2.cpp b/python/handwritten/geometry/Pose2.cpp index 5b4b4731b..aaf17f812 100644 --- a/python/handwritten/geometry/Pose2.cpp +++ b/python/handwritten/geometry/Pose2.cpp @@ -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(compose_overloads, Pose2::compose, 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_from_overloads, Pose2::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transformTo, 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(range_overloads, Pose2::range, 1, 3) @@ -61,9 +61,9 @@ void exportPose2(){ // .def("dim", &Pose2::dim) // .def("retract", &Pose2::retract) - .def("transform_to", &Pose2::transform_to, + .def("transformTo", &Pose2::transformTo, transform_to_overloads(args("point", "H1", "H2"))) - .def("transform_from", &Pose2::transform_from, + .def("transformFrom", &Pose2::transformFrom, transform_to_overloads(args("point", "H1", "H2"))) .def("x", &Pose2::x) diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp index 551f2f60c..cbddab0a9 100644 --- a/python/handwritten/geometry/Pose3.cpp +++ b/python/handwritten/geometry/Pose3.cpp @@ -30,8 +30,8 @@ using namespace gtsam; 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(transform_to_overloads, Pose3::transform_to, 1, 3) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transformTo, 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(translation_overloads, Pose3::translation, 0, 1) 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(){ - // function pointers to desambiguate transform_to() calls - Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transform_to; - Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transform_to; + // function pointers to desambiguate transformTo() calls + Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transformTo; + Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transformTo; // function pointers to desambiguate compose() calls Pose3 (Pose3::*compose1)(const Pose3 &g) 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) .staticmethod("identity") .def("matrix", &Pose3::matrix) - .def("transform_from", &Pose3::transform_from, + .def("transformFrom", &Pose3::transformFrom, transform_from_overloads(args("point", "H1", "H2"))) - .def("transform_to", transform_to1, transform_to_overloads(args("point", "H1", "H2"))) - .def("transform_to", transform_to2) + .def("transformTo", transform_to1, transform_to_overloads(args("point", "H1", "H2"))) + .def("transformTo", transform_to2) .def("x", &Pose3::x) .def("y", &Pose3::y) .def("z", &Pose3::z) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 161e6976b..d9a013a60 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -228,7 +228,7 @@ TEST(ExpressionFactor, Shallow) { Point3_ p_(2); // Construct expression, concise evrsion - Point2_ expression = project(transform_to(x_, p_)); + Point2_ expression = project(transformTo(x_, p_)); // Get and check keys and dims KeyVector keys; @@ -288,7 +288,7 @@ TEST(ExpressionFactor, tree) { Cal3_S2_ K(3); // 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_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); @@ -301,7 +301,7 @@ TEST(ExpressionFactor, tree) { // Concise version ExpressionFactor 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_LONGS_EQUAL(2, f2.dim()); boost::shared_ptr gf2 = f2.linearize(values); @@ -461,7 +461,7 @@ TEST(ExpressionFactor, tree_finite_differences) { Cal3_S2_ K(3); // 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_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); diff --git a/timing/timeCameraExpression.cpp b/timing/timeCameraExpression.cpp index 0909ac7a5..07eeb1bcb 100644 --- a/timing/timeCameraExpression.cpp +++ b/timing/timeCameraExpression.cpp @@ -68,7 +68,7 @@ int main() { // 20.3 musecs/call NonlinearFactor::shared_ptr f2 = boost::make_shared >(model, z, - uncalibrate(K, project(transform_to(x, p)))); + uncalibrate(K, project(transformTo(x, p)))); time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values); // ExpressionFactor ternary @@ -93,7 +93,7 @@ int main() { // 16.0 musecs/call NonlinearFactor::shared_ptr g2 = boost::make_shared >(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); // ExpressionFactor, optimized diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index a5740a1f1..ec5fc3fa5 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -156,7 +156,7 @@ int main(int argc, char *argv[]) { Rot2 measuredBearing = measurement->measured().bearing(); double measuredRange = measurement->measured().range(); newVariables.insert(lmKey, - pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); + pose.transformFrom(measuredBearing.rotate(Point2(measuredRange, 0.0)))); } } else diff --git a/timing/timeOneCameraExpression.cpp b/timing/timeOneCameraExpression.cpp index 53510cfa5..73b2ff55f 100644 --- a/timing/timeOneCameraExpression.cpp +++ b/timing/timeOneCameraExpression.cpp @@ -50,7 +50,7 @@ int main() { #ifdef TERNARY (model, z, project3(x, p, K)); #else - (model, z, uncalibrate(K, project(transform_to(x, p)))); + (model, z, uncalibrate(K, project(transformTo(x, p)))); #endif time("timing:", f, values); diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp index 33680e813..0475253ec 100644 --- a/timing/timeSFMBALcamTnav.cpp +++ b/timing/timeSFMBALcamTnav.cpp @@ -42,8 +42,8 @@ int main(int argc, char* argv[]) { Point3_ nav_point_(P(j)); graph.addExpressionFactor( gNoiseModel, z, - uncalibrate(calibration_, // now using transform_from !!!: - project(transform_from(camTnav_, nav_point_)))); + uncalibrate(calibration_, // now using transformFrom !!!: + project(transformFrom(camTnav_, nav_point_)))); } } diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp index dad1edd4e..fc9568626 100644 --- a/timing/timeSFMBALnavTcam.cpp +++ b/timing/timeSFMBALnavTcam.cpp @@ -43,7 +43,7 @@ int main(int argc, char* argv[]) { graph.addExpressionFactor( gNoiseModel, z, uncalibrate(calibration_, - project(transform_to(navTcam_, nav_point_)))); + project(transformTo(navTcam_, nav_point_)))); } } diff --git a/timing/timeSFMExpressions.cpp b/timing/timeSFMExpressions.cpp index 65241b63b..8ab949be7 100644 --- a/timing/timeSFMExpressions.cpp +++ b/timing/timeSFMExpressions.cpp @@ -61,7 +61,7 @@ int main() { #ifdef TERNARY (model, z, project3(x[i], p[j], K)); #else - (model, z, uncalibrate(K, project(transform_to(x[i], p[j])))); + (model, z, uncalibrate(K, project(transformTo(x[i], p[j])))); #endif graph.push_back(f); }