From c31298d367ba1ae091d2919cfc78f4e3c0213f9d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 7 Jul 2022 12:31:27 -0400 Subject: [PATCH] Capitalize Identity trait since it is static --- gtsam/base/Group.h | 8 +-- gtsam/base/Lie.h | 2 +- gtsam/base/ProductLieGroup.h | 2 +- gtsam/base/VectorSpace.h | 4 +- gtsam/base/tests/testGroup.cpp | 2 +- gtsam/basis/ParameterMatrix.h | 4 +- gtsam/basis/tests/testParameterMatrix.cpp | 2 +- gtsam/geometry/Cyclic.h | 2 +- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/PinholePose.h | 2 +- gtsam/geometry/Pose2.h | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/Rot2.h | 4 +- gtsam/geometry/Rot3.h | 2 +- gtsam/geometry/SOn.h | 4 +- gtsam/geometry/Similarity2.cpp | 2 +- gtsam/geometry/Similarity2.h | 2 +- gtsam/geometry/Similarity3.cpp | 2 +- gtsam/geometry/Similarity3.h | 2 +- gtsam/geometry/SphericalCamera.h | 6 +-- gtsam/geometry/StereoPoint2.h | 2 +- gtsam/geometry/geometry.i | 20 +++---- gtsam/geometry/tests/testPose2.cpp | 2 +- gtsam/geometry/tests/testPose3.cpp | 12 ++--- gtsam/geometry/tests/testSimilarity3.cpp | 6 +-- gtsam/navigation/ImuBias.h | 2 +- gtsam/navigation/navigation.i | 2 +- gtsam/nonlinear/tests/testExpression.cpp | 4 +- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 4 +- .../tests/testSmartProjectionRigFactor.cpp | 54 +++++++++---------- .../dynamics/tests/testVelocityConstraint.cpp | 4 +- .../examples/GncPoseAveragingExample.cpp | 2 +- .../examples/ISAM2_SmartFactorStereo_IMU.cpp | 6 +-- gtsam_unstable/geometry/Pose3Upright.h | 2 +- gtsam_unstable/gtsam_unstable.i | 2 +- .../tests/testLocalOrientedPlane3Factor.cpp | 12 ++--- .../slam/tests/testPartialPriorFactor.cpp | 2 +- .../slam/tests/testPoseToPointFactor.cpp | 8 +-- ...martProjectionPoseFactorRollingShutter.cpp | 34 ++++++------ .../tests/testSmartStereoFactor_iSAM2.cpp | 8 +-- .../testSmartStereoProjectionFactorPP.cpp | 14 ++--- tests/testExpressionFactor.cpp | 2 +- tests/testManifold.cpp | 2 +- timing/timeShonanFactor.cpp | 6 +-- 44 files changed, 135 insertions(+), 135 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 39541416ee..89fa8248c1 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -95,7 +95,7 @@ template struct MultiplicativeGroupTraits { typedef group_tag structure_category; typedef multiplicative_group_tag group_flavor; - static Class Identity() { return Class::identity(); } + static Class Identity() { return Class::Identity(); } static Class Compose(const Class &g, const Class & h) { return g * h;} static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} static Class Inverse(const Class &g) { return g.inverse();} @@ -111,7 +111,7 @@ template struct AdditiveGroupTraits { typedef group_tag structure_category; typedef additive_group_tag group_flavor; - static Class Identity() { return Class::identity(); } + static Class Identity() { return Class::Identity(); } static Class Compose(const Class &g, const Class & h) { return g + h;} static Class Between(const Class &g, const Class & h) { return h - g;} static Class Inverse(const Class &g) { return -g;} @@ -147,7 +147,7 @@ class DirectProduct: public std::pair { DirectProduct(const G& g, const H& h):std::pair(g,h) {} // identity - static DirectProduct identity() { return DirectProduct(); } + static DirectProduct Identity() { return DirectProduct(); } DirectProduct operator*(const DirectProduct& other) const { return DirectProduct(traits::Compose(this->first, other.first), @@ -181,7 +181,7 @@ class DirectSum: public std::pair { DirectSum(const G& g, const H& h):std::pair(g,h) {} // identity - static DirectSum identity() { return DirectSum(); } + static DirectSum Identity() { return DirectSum(); } DirectSum operator+(const DirectSum& other) const { return DirectSum(g()+other.g(), h()+other.h()); diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index cb8e7d017e..c4eba5deb8 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -177,7 +177,7 @@ struct LieGroupTraits: GetDimensionImpl { /// @name Group /// @{ typedef multiplicative_group_tag group_flavor; - static Class Identity() { return Class::identity();} + static Class Identity() { return Class::Identity();} /// @} /// @name Manifold diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index a2dcf738e0..6689c11d60 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -48,7 +48,7 @@ class ProductLieGroup: public std::pair { /// @name Group /// @{ typedef multiplicative_group_tag group_flavor; - static ProductLieGroup identity() {return ProductLieGroup();} + static ProductLieGroup Identity() {return ProductLieGroup();} ProductLieGroup operator*(const ProductLieGroup& other) const { return ProductLieGroup(traits::Compose(this->first,other.first), diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 43644b5c46..f7809f5968 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -169,7 +169,7 @@ struct HasVectorSpacePrereqs { Vector v; BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) { - p = Class::identity(); // identity + p = Class::Identity(); // identity q = p + p; // addition q = p - p; // subtraction v = p.vector(); // conversion to vector @@ -192,7 +192,7 @@ struct VectorSpaceTraits: VectorSpaceImpl { /// @name Group /// @{ typedef additive_group_tag group_flavor; - static Class Identity() { return Class::identity();} + static Class Identity() { return Class::Identity();} /// @} /// @name Manifold diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp index 976dee6976..7e8cb7821c 100644 --- a/gtsam/base/tests/testGroup.cpp +++ b/gtsam/base/tests/testGroup.cpp @@ -29,7 +29,7 @@ class Symmetric: private Eigen::PermutationMatrix { Eigen::PermutationMatrix(P) { } public: - static Symmetric identity() { return Symmetric(); } + static Symmetric Identity() { return Symmetric(); } Symmetric() { Eigen::PermutationMatrix::setIdentity(); } diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h index eddcbfeaec..3f830ec516 100644 --- a/gtsam/basis/ParameterMatrix.h +++ b/gtsam/basis/ParameterMatrix.h @@ -189,9 +189,9 @@ class ParameterMatrix { * NOTE: The size at compile time is unknown so this identity is zero * length and thus not valid. */ - inline static ParameterMatrix identity() { + inline static ParameterMatrix Identity() { // throw std::runtime_error( - // "ParameterMatrix::identity(): Don't use this function"); + // "ParameterMatrix::Identity(): Don't use this function"); return ParameterMatrix(0); } diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp index ec62e8eeab..ae2e8e1116 100644 --- a/gtsam/basis/tests/testParameterMatrix.cpp +++ b/gtsam/basis/tests/testParameterMatrix.cpp @@ -133,7 +133,7 @@ TEST(ParameterMatrix, VectorSpace) { // vector EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); // identity - EXPECT(assert_equal(ParameterMatrix::identity(), + EXPECT(assert_equal(ParameterMatrix::Identity(), ParameterMatrix(Matrix::Zero(M, 0)))); } diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 065cd01407..a503a6072b 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -38,7 +38,7 @@ class Cyclic { /// Default constructor yields identity Cyclic():i_(0) { } - static Cyclic identity() { return Cyclic();} + static Cyclic Identity() { return Cyclic();} /// Cast to size_t operator size_t() const { diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c20e908191..e5a8eec7a1 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -213,7 +213,7 @@ class PinholeCamera: public PinholeBaseK { } /// for Canonical - static PinholeCamera identity() { + static PinholeCamera Identity() { return PinholeCamera(); // assumes that the default constructor is valid } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 7b92be5d50..ce393e45fb 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -412,7 +412,7 @@ class PinholePose: public PinholeBaseK { } /// for Canonical - static PinholePose identity() { + static PinholePose Identity() { return PinholePose(); // assumes that the default constructor is valid } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 466c5a42ad..008e6525d6 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -119,7 +119,7 @@ class Pose2: public LieGroup { /// @{ /// identity for group operation - inline static Pose2 identity() { return Pose2(); } + inline static Pose2 Identity() { return Pose2(); } /// inverse GTSAM_EXPORT Pose2 inverse() const; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 33fb55250f..6dec7f74fc 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -103,7 +103,7 @@ class GTSAM_EXPORT Pose3: public LieGroup { /// @{ /// identity for group operation - static Pose3 identity() { + static Pose3 Identity() { return Pose3(); } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 2690ca2481..be8d11cdab 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -107,8 +107,8 @@ namespace gtsam { /// @name Group /// @{ - /** identity */ - inline static Rot2 identity() { return Rot2(); } + /** Identity */ + inline static Rot2 Identity() { return Rot2(); } /** The inverse rotation - negative angle */ Rot2 inverse() const { return Rot2(c_, -s_);} diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 01ca7778c6..800ce4cdf9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -297,7 +297,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// @{ /// identity rotation for group operation - inline static Rot3 identity() { + inline static Rot3 Identity() { return Rot3(); } diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index af0e7a3cfb..7d1e6db7af 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -178,13 +178,13 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// SO identity for N >= 2 template > - static SO identity() { + static SO Identity() { return SO(); } /// SO identity for N == Eigen::Dynamic template > - static SO identity(size_t n = 0) { + static SO Identity(size_t n = 0) { return SO(n); } diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp index 4ed3351f8b..4b8cfd5818 100644 --- a/gtsam/geometry/Similarity2.cpp +++ b/gtsam/geometry/Similarity2.cpp @@ -134,7 +134,7 @@ void Similarity2::print(const std::string& s) const { << std::endl; } -Similarity2 Similarity2::identity() { return Similarity2(); } +Similarity2 Similarity2::Identity() { return Similarity2(); } Similarity2 Similarity2::operator*(const Similarity2& S) const { return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h index 05f10d1493..e72cda484c 100644 --- a/gtsam/geometry/Similarity2.h +++ b/gtsam/geometry/Similarity2.h @@ -83,7 +83,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup { /// @{ /// Return an identity transform - static Similarity2 identity(); + static Similarity2 Identity(); /// Composition Similarity2 operator*(const Similarity2& S) const; diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index 7fde974c55..1461617965 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -122,7 +122,7 @@ void Similarity3::print(const std::string& s) const { std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl; } -Similarity3 Similarity3::identity() { +Similarity3 Similarity3::Identity() { return Similarity3(); } Similarity3 Similarity3::operator*(const Similarity3& S) const { diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 845d4c810d..76d2bf5361 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -84,7 +84,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { /// @{ /// Return an identity transform - static Similarity3 identity(); + static Similarity3 Identity(); /// Composition Similarity3 operator*(const Similarity3& S) const; diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index 4880423d32..adcbe46a5f 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -88,7 +88,7 @@ class GTSAM_EXPORT SphericalCamera { /// Default constructor SphericalCamera() - : pose_(Pose3::identity()), emptyCal_(boost::make_shared()) {} + : pose_(Pose3()), emptyCal_(boost::make_shared()) {} /// Constructor with pose explicit SphericalCamera(const Pose3& pose) @@ -198,9 +198,9 @@ class GTSAM_EXPORT SphericalCamera { } /// for Canonical - static SphericalCamera identity() { + static SphericalCamera Identity() { return SphericalCamera( - Pose3::identity()); // assumes that the default constructor is valid + Pose3::Identity()); // assumes that the default constructor is valid } /// for Linear Triangulation diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 9eef01577d..8c9197fd28 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -71,7 +71,7 @@ class GTSAM_EXPORT StereoPoint2 { /// @{ /// identity - inline static StereoPoint2 identity() { + inline static StereoPoint2 Identity() { return StereoPoint2(); } diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 92d9ec0a39..a293c6327e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -16,7 +16,7 @@ class Point2 { bool equals(const gtsam::Point2& point, double tol) const; // Group - static gtsam::Point2 identity(); + static gtsam::Point2 Identity(); // Standard Interface double x() const; @@ -73,7 +73,7 @@ class StereoPoint2 { bool equals(const gtsam::StereoPoint2& point, double tol) const; // Group - static gtsam::StereoPoint2 identity(); + static gtsam::StereoPoint2 Identity(); gtsam::StereoPoint2 inverse() const; gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; @@ -115,7 +115,7 @@ class Point3 { bool equals(const gtsam::Point3& p, double tol) const; // Group - static gtsam::Point3 identity(); + static gtsam::Point3 Identity(); // Standard Interface Vector vector() const; @@ -149,7 +149,7 @@ class Rot2 { bool equals(const gtsam::Rot2& rot, double tol) const; // Group - static gtsam::Rot2 identity(); + static gtsam::Rot2 Identity(); gtsam::Rot2 inverse(); gtsam::Rot2 compose(const gtsam::Rot2& p2) const; gtsam::Rot2 between(const gtsam::Rot2& p2) const; @@ -198,7 +198,7 @@ class SO3 { bool equals(const gtsam::SO3& other, double tol) const; // Group - static gtsam::SO3 identity(); + static gtsam::SO3 Identity(); gtsam::SO3 inverse() const; gtsam::SO3 between(const gtsam::SO3& R) const; gtsam::SO3 compose(const gtsam::SO3& R) const; @@ -228,7 +228,7 @@ class SO4 { bool equals(const gtsam::SO4& other, double tol) const; // Group - static gtsam::SO4 identity(); + static gtsam::SO4 Identity(); gtsam::SO4 inverse() const; gtsam::SO4 between(const gtsam::SO4& Q) const; gtsam::SO4 compose(const gtsam::SO4& Q) const; @@ -258,7 +258,7 @@ class SOn { bool equals(const gtsam::SOn& other, double tol) const; // Group - static gtsam::SOn identity(); + static gtsam::SOn Identity(); gtsam::SOn inverse() const; gtsam::SOn between(const gtsam::SOn& Q) const; gtsam::SOn compose(const gtsam::SOn& Q) const; @@ -322,7 +322,7 @@ class Rot3 { bool equals(const gtsam::Rot3& rot, double tol) const; // Group - static gtsam::Rot3 identity(); + static gtsam::Rot3 Identity(); gtsam::Rot3 inverse() const; gtsam::Rot3 compose(const gtsam::Rot3& p2) const; gtsam::Rot3 between(const gtsam::Rot3& p2) const; @@ -380,7 +380,7 @@ class Pose2 { bool equals(const gtsam::Pose2& pose, double tol) const; // Group - static gtsam::Pose2 identity(); + static gtsam::Pose2 Identity(); gtsam::Pose2 inverse() const; gtsam::Pose2 compose(const gtsam::Pose2& p2) const; gtsam::Pose2 between(const gtsam::Pose2& p2) const; @@ -444,7 +444,7 @@ class Pose3 { bool equals(const gtsam::Pose3& pose, double tol) const; // Group - static gtsam::Pose3 identity(); + static gtsam::Pose3 Identity(); gtsam::Pose3 inverse() const; gtsam::Pose3 inverse(Eigen::Ref H) const; gtsam::Pose3 compose(const gtsam::Pose3& pose) const; diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index de779cc750..88c00a2e97 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -902,7 +902,7 @@ TEST(Pose2 , TransformCovariance3) { /* ************************************************************************* */ TEST(Pose2, Print) { - Pose2 pose(Rot2::identity(), Point2(1, 2)); + Pose2 pose(Rot2::Identity(), Point2(1, 2)); // Generate the expected output string s = "Planar Pose"; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index e1d3d5ea22..78a4af799e 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1133,7 +1133,7 @@ Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { retu TEST(Pose3, interpolateJacobians) { { - Pose3 X = Pose3::identity(); + Pose3 X = Pose3::Identity(); Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0)); double t = 0.5; Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation @@ -1147,10 +1147,10 @@ TEST(Pose3, interpolateJacobians) { EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); } { - Pose3 X = Pose3::identity(); - Pose3 Y(Rot3::identity(), Point3(1, 0, 0)); + Pose3 X = Pose3::Identity(); + Pose3 Y(Rot3::Identity(), Point3(1, 0, 0)); double t = 0.3; - Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0)); + Pose3 expectedPoseInterp(Rot3::Identity(), Point3(0.3, 0, 0)); Matrix actualJacobianX, actualJacobianY; EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); @@ -1161,7 +1161,7 @@ TEST(Pose3, interpolateJacobians) { EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); } { - Pose3 X = Pose3::identity(); + Pose3 X = Pose3::Identity(); Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0)); double t = 0.5; Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); @@ -1204,7 +1204,7 @@ TEST(Pose3, Create) { /* ************************************************************************* */ TEST(Pose3, Print) { - Pose3 pose(Rot3::identity(), Point3(1, 2, 3)); + Pose3 pose(Rot3::Identity(), Point3(1, 2, 3)); // Generate the expected output std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n"; diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 7a134f6efd..fad24f7431 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -203,11 +203,11 @@ TEST(Similarity3, ExpLogMap) { Vector7 zeros; zeros << 0, 0, 0, 0, 0, 0, 0; - Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity()); + Vector7 logIdentity = Similarity3::Logmap(Similarity3::Identity()); EXPECT(assert_equal(zeros, logIdentity)); Similarity3 expZero = Similarity3::Expmap(zeros); - Similarity3 ident = Similarity3::identity(); + Similarity3 ident = Similarity3::Identity(); EXPECT(assert_equal(expZero, ident)); // Compare to matrix exponential, using expm in Lie.h @@ -391,7 +391,7 @@ TEST(Similarity3, Optimization) { NonlinearFactorGraph graph; graph.addPrior(key, prior, model); - // Create initial estimate with identity transform + // Create initial estimate with Identity transform Values initial; initial.insert(key, Similarity3()); diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 9346a4a777..936ae77829 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -105,7 +105,7 @@ class GTSAM_EXPORT ConstantBias { /// @{ /** identity for group operation */ - static ConstantBias identity() { + static ConstantBias Identity() { return ConstantBias(); } diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 6ede1645f3..2477f1288b 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -17,7 +17,7 @@ class ConstantBias { bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; // Group - static gtsam::imuBias::ConstantBias identity(); + static gtsam::imuBias::ConstantBias Identity(); // Operator Overloads gtsam::imuBias::ConstantBias operator-() const; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 92f4998a20..9987cca3f9 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -122,7 +122,7 @@ class Class : public Point3 { enum {dimension = 3}; using Point3::Point3; const Vector3& vector() const { return *this; } - inline static Class identity() { return Class(0,0,0); } + inline static Class Identity() { return Class(0,0,0); } double norm(OptionalJacobian<1,3> H = boost::none) const { return norm3(*this, H); } @@ -285,7 +285,7 @@ TEST(Expression, compose2) { // Test compose with one arguments referring to constant rotation. TEST(Expression, compose3) { // Create expression - Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R1(Rot3::Identity()), R2(3); Rot3_ R3 = R1 * R2; // Check keys diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 35c7504081..502c62c11c 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -217,7 +217,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { // Setup prior factors // Note: If x0 is too far away from the origin (e.g. x=100) this test can fail. - Pose3 x0(Rot3::identity(), Vector3(10, -1, 1)); + Pose3 x0(Rot3::Identity(), Vector3(10, -1, 1)); auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); graph.addPrior(X(0), x0, x0_noise); @@ -241,7 +241,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { // Initial values // Just offset the initial pose by 1m. This is what we are trying to optimize. Values initialEstimate; - Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); + Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1,0,0))); initialEstimate.insert(P(1), p1); initialEstimate.insert(P(2), p2); initialEstimate.insert(X(0), x0_initial); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index b4876b27ea..b142b20091 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -69,7 +69,7 @@ SmartProjectionParams params( TEST(SmartProjectionRigFactor, Constructor) { using namespace vanillaRig; boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); } @@ -89,7 +89,7 @@ TEST(SmartProjectionRigFactor, Constructor2) { TEST(SmartProjectionRigFactor, Constructor3) { using namespace vanillaRig; boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); factor1->add(measurement1, x1, cameraId1); @@ -99,7 +99,7 @@ TEST(SmartProjectionRigFactor, Constructor3) { TEST(SmartProjectionRigFactor, Constructor4) { using namespace vanillaRig; boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); SmartProjectionParams params2( gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors @@ -112,7 +112,7 @@ TEST(SmartProjectionRigFactor, Constructor4) { TEST(SmartProjectionRigFactor, Equals) { using namespace vanillaRig; boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); @@ -140,7 +140,7 @@ TEST(SmartProjectionRigFactor, noiseless) { Point2 level_uv_right = level_camera_right.project(landmark1); boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); SmartRigFactor factor(model, cameraRig, params); factor.add(level_uv, x1); // both taken from the same camera @@ -197,7 +197,7 @@ TEST(SmartProjectionRigFactor, noisy) { using namespace vanillaRig; boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); // Project two landmarks into two cameras Point2 pixelError(0.2, 0.2); @@ -323,7 +323,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1)); - Pose3 body_T_sensor3 = Pose3::identity(); + Pose3 body_T_sensor3 = Pose3(); boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(body_T_sensor1, sharedK)); @@ -407,7 +407,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK2)); + cameraRig->push_back(Camera(Pose3(), sharedK2)); // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -495,7 +495,7 @@ TEST(SmartProjectionRigFactor, Factors) { FastVector cameraIds{0, 0}; boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( model, cameraRig, params); @@ -578,7 +578,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { // create smart factor boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( new SmartRigFactor(model, cameraRig, params)); @@ -646,7 +646,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { params.setEnableEPI(false); boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -717,7 +717,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -783,7 +783,7 @@ TEST(SmartProjectionRigFactor, CheckHessian) { params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -859,7 +859,7 @@ TEST(SmartProjectionRigFactor, Hessian) { measurements_cam1.push_back(cam2_uv1); boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK2)); + cameraRig->push_back(Camera(Pose3(), sharedK2)); FastVector cameraIds{0, 0}; SmartProjectionParams params( @@ -889,7 +889,7 @@ TEST(SmartProjectionRigFactor, Hessian) { TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK)); + cameraRig->push_back(Camera(Pose3(), sharedBundlerK)); SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); @@ -917,7 +917,7 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) { KeyVector views{x1, x2, x3}; boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK)); + cameraRig->push_back(Camera(Pose3(), sharedBundlerK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -988,7 +988,7 @@ TEST(SmartProjectionRigFactor, KeyVector keys{x1, x2, x3, x1}; boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -1116,7 +1116,7 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { // create inputs KeyVector keys{x1, x2, x3}; boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; FastVector cameraIdsRedundant{0, 0, 0, 0}; @@ -1195,11 +1195,11 @@ TEST(SmartProjectionRigFactor, timing) { // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); - Rot3 R = Rot3::identity(); + Rot3 R = Rot3::Identity(); Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); - Pose3 body_P_sensorId = Pose3::identity(); + Pose3 body_P_sensorId = Pose3(); boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); @@ -1267,7 +1267,7 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) { keys.push_back(x3); boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), emptyK)); + cameraRig->push_back(Camera(Pose3(), emptyK)); SmartProjectionParams params( gtsam::HESSIAN, @@ -1330,10 +1330,10 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) { /* *************************************************************************/ TEST(SmartProjectionFactorP, timing_sphericalCamera) { // create common data - Rot3 R = Rot3::identity(); + Rot3 R = Rot3::Identity(); Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); - Pose3 body_P_sensorId = Pose3::identity(); + Pose3 body_P_sensorId = Pose3(); Point3 landmark1(0, 0, 10); // create spherical data @@ -1423,7 +1423,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) { boost::shared_ptr>> cameraRig( new CameraSet>()); // single camera in the rig - cameraRig->push_back(PinholePose(Pose3::identity(), sharedK)); + cameraRig->push_back(PinholePose(Pose3(), sharedK)); SmartRigFactor::shared_ptr smartFactor1( new SmartRigFactor(model, cameraRig, params)); @@ -1461,7 +1461,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) { boost::shared_ptr>> cameraRig( new CameraSet>()); // single camera in the rig - cameraRig->push_back(PinholePose(Pose3::identity(), canonicalK)); + cameraRig->push_back(PinholePose(Pose3(), canonicalK)); SmartRigFactor::shared_ptr smartFactor1( new SmartRigFactor(model, cameraRig, params)); @@ -1498,7 +1498,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) { boost::shared_ptr>> cameraRig( new CameraSet>()); // single camera in the rig - cameraRig->push_back(PinholePose(Pose3::identity(), canonicalK)); + cameraRig->push_back(PinholePose(Pose3(), canonicalK)); SmartRigFactor::shared_ptr smartFactor1( new SmartRigFactor(model, cameraRig, params)); @@ -1537,7 +1537,7 @@ TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) { boost::shared_ptr> cameraRig( new CameraSet()); // single camera in the rig - cameraRig->push_back(SphericalCamera(Pose3::identity(), emptyK)); + cameraRig->push_back(SphericalCamera(Pose3(), emptyK)); // TRIANGULATION TEST WITH DEFAULT RANK TOL { // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 6d82061770..b60b2f3ae9 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2; const double dt = 1.0; PoseRTV origin, - pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)), + pose1(Point3(0.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)), - pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)); + pose2(Point3(1.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) { diff --git a/gtsam_unstable/examples/GncPoseAveragingExample.cpp b/gtsam_unstable/examples/GncPoseAveragingExample.cpp index ad96934c8b..730d342f07 100644 --- a/gtsam_unstable/examples/GncPoseAveragingExample.cpp +++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp @@ -53,7 +53,7 @@ int main(int argc, char** argv){ normal_distribution normalInliers(0.0, 0.05); Values initial; - initial.insert(0, Pose3::identity()); // identity pose as initialization + initial.insert(0, Pose3()); // identity pose as initialization // create ground truth pose Vector6 poseGtVector; diff --git a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp index c0a102d111..16eda23d4a 100644 --- a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -129,8 +129,8 @@ int main(int argc, char* argv[]) { // Pose prior - at identity auto priorPoseNoise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished()); - graph.addPrior(X(1), Pose3::identity(), priorPoseNoise); - initialEstimate.insert(X(0), Pose3::identity()); + graph.addPrior(X(1), Pose3::Identity(), priorPoseNoise); + initialEstimate.insert(X(0), Pose3::Identity()); // Bias prior graph.addPrior(B(1), imu.priorImuBias, @@ -157,7 +157,7 @@ int main(int argc, char* argv[]) { if (frame != lastFrame || in.eof()) { cout << "Running iSAM for frame: " << lastFrame << "\n"; - initialEstimate.insert(X(lastFrame), Pose3::identity()); + initialEstimate.insert(X(lastFrame), Pose3::Identity()); initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0)); initialEstimate.insert(B(lastFrame), imu.prevBias); diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index d833c9cf46..037a6cc9da 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -95,7 +95,7 @@ class GTSAM_UNSTABLE_EXPORT Pose3Upright { /// @{ /// identity for group operation - static Pose3Upright identity() { return Pose3Upright(); } + static Pose3Upright Identity() { return Pose3Upright(); } /// inverse transformation with derivatives Pose3Upright inverse(boost::optional H1=boost::none) const; diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 08cd45e186..c6dbd4ab62 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -130,7 +130,7 @@ class Pose3Upright { gtsam::Pose3Upright retract(Vector v) const; Vector localCoordinates(const gtsam::Pose3Upright& p2) const; - static gtsam::Pose3Upright identity(); + static gtsam::Pose3Upright Identity(); gtsam::Pose3Upright inverse() const; gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const; gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const; diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index b97d56c7ea..c2b526ecde 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -44,8 +44,8 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement // does not fully constrain the pose - Pose3 init_pose = Pose3::identity(); - Pose3 anchor_pose = Pose3::identity(); + Pose3 init_pose = Pose3::Identity(); + Pose3 anchor_pose = Pose3::Identity(); graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001)); @@ -89,7 +89,7 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement // does not fully constrain the pose - Pose3 init_pose = Pose3::identity(); + Pose3 init_pose = Pose3::Identity(); graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); // Add two landmark measurements, differing in angle @@ -180,8 +180,8 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) { NonlinearFactorGraph graph; // Setup prior factors - Pose3 x0(Rot3::identity(), Vector3(100, 30, 10)); // the "sensor pose" - Pose3 x1(Rot3::identity(), Vector3(90, 40, 5) ); // the "anchor pose" + Pose3 x0(Rot3::Identity(), Vector3(100, 30, 10)); // the "sensor pose" + Pose3 x1(Rot3::Identity(), Vector3(90, 40, 5) ); // the "anchor pose" auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01); @@ -213,7 +213,7 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) { // Initial values // Just offset the initial pose by 1m. This is what we are trying to optimize. Values initialEstimate; - Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1, 0, 0))); + Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1, 0, 0))); initialEstimate.insert(P(1), p1_in_x1); initialEstimate.insert(P(2), p2_in_x1); initialEstimate.insert(X(0), x0_initial); diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp index 7ba8a0d04b..615ed13aae 100644 --- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -173,7 +173,7 @@ TEST(PartialPriorFactor, Constructors3) { /* ************************************************************************* */ TEST(PartialPriorFactor, JacobianAtIdentity3) { Key poseKey(1); - Pose3 measurement = Pose3::identity(); + Pose3 measurement = Pose3::Identity(); SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25); TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model); diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp b/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp index 5aaaaec531..7345e23081 100644 --- a/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp @@ -16,7 +16,7 @@ using namespace gtsam::noiseModel; /* ************************************************************************* */ // Verify zero error when there is no noise TEST(PoseToPointFactor, errorNoiseless_2D) { - Pose2 pose = Pose2::identity(); + Pose2 pose = Pose2::Identity(); Point2 point(1.0, 2.0); Point2 noise(0.0, 0.0); Point2 measured = point + noise; @@ -33,7 +33,7 @@ TEST(PoseToPointFactor, errorNoiseless_2D) { /* ************************************************************************* */ // Verify expected error in test scenario TEST(PoseToPointFactor, errorNoise_2D) { - Pose2 pose = Pose2::identity(); + Pose2 pose = Pose2::Identity(); Point2 point(1.0, 2.0); Point2 noise(-1.0, 0.5); Point2 measured = point + noise; @@ -86,7 +86,7 @@ TEST(PoseToPointFactor, jacobian_2D) { /* ************************************************************************* */ // Verify zero error when there is no noise TEST(PoseToPointFactor, errorNoiseless_3D) { - Pose3 pose = Pose3::identity(); + Pose3 pose = Pose3::Identity(); Point3 point(1.0, 2.0, 3.0); Point3 noise(0.0, 0.0, 0.0); Point3 measured = point + noise; @@ -103,7 +103,7 @@ TEST(PoseToPointFactor, errorNoiseless_3D) { /* ************************************************************************* */ // Verify expected error in test scenario TEST(PoseToPointFactor, errorNoise_3D) { - Pose3 pose = Pose3::identity(); + Pose3 pose = Pose3::Identity(); Point3 point(1.0, 2.0, 3.0); Point3 noise(-1.0, 0.5, 0.3); Point3 measured = point + noise; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index b5962d777b..b88a27c6ca 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -88,7 +88,7 @@ typedef SmartProjectionPoseFactorRollingShutter> TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig, params)); } @@ -97,7 +97,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPoseRS; boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); params.setRankTolerance(rankTol); SmartFactorRS factor1(model, cameraRig, params); } @@ -106,7 +106,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor); @@ -230,7 +230,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { // Project two landmarks into two cameras Point2 level_uv = cam1.project(landmark1); Point2 level_uv_right = cam2.project(landmark1); - Pose3 body_P_sensorId = Pose3::identity(); + Pose3 body_P_sensorId = Pose3::Identity(); boost::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(body_P_sensorId, sharedK)); @@ -405,7 +405,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor3); boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -480,7 +480,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { boost::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(body_P_sensor, sharedK)); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -633,11 +633,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); - Rot3 R = Rot3::identity(); + Rot3 R = Rot3::Identity(); Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); - Pose3 body_P_sensorId = Pose3::identity(); + Pose3 body_P_sensorId = Pose3::Identity(); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); @@ -747,7 +747,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { params.setEnableEPI(true); boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); @@ -816,7 +816,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setEnableEPI(false); boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); @@ -894,7 +894,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setEnableEPI(false); boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -961,7 +961,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -1102,7 +1102,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor1); boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -1261,7 +1261,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.at(0)); // we readd the first interp factor boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), sharedK)); + cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -1331,11 +1331,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors - Rot3 R = Rot3::identity(); + Rot3 R = Rot3::Identity(); Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); - Pose3 body_P_sensorId = Pose3::identity(); + Pose3 body_P_sensorId = Pose3::Identity(); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); @@ -1431,7 +1431,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setRankTolerance(0.1); boost::shared_ptr cameraRig(new Cameras()); - cameraRig->push_back(Camera(Pose3::identity(), emptyK)); + cameraRig->push_back(Camera(Pose3::Identity(), emptyK)); SmartFactorRS_spherical::shared_ptr smartFactor1( new SmartFactorRS_spherical(model, cameraRig, params)); diff --git a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp index 107defb4c9..98b18371fc 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp @@ -198,10 +198,10 @@ TEST(testISAM2SmartFactor, Stereo_Batch) { // prior, for the first keyframe: if (kf_id == 0) { - batch_graph.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise); + batch_graph.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise); } - batch_values.insert(X(kf_id), Pose3::identity()); + batch_values.insert(X(kf_id), Pose3::Identity()); } LevenbergMarquardtParams parameters; @@ -267,7 +267,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) { // Storage of smart factors: std::map smartFactors; - Pose3 lastKeyframePose = Pose3::identity(); + Pose3 lastKeyframePose = Pose3::Identity(); // Run one timestep at once: for (const auto &entries : dataset) { @@ -307,7 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) { // prior, for the first keyframe: if (kf_id == 0) { - newFactors.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise); + newFactors.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise); } // 2) Run iSAM2: diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp index c71c19038d..1dbd256664 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp @@ -181,8 +181,8 @@ TEST_UNSAFE( SmartStereoProjectionFactorPP, noiseless_error_identityExtrinsics ) Values values; values.insert(x1, w_Pose_cam1); values.insert(x2, w_Pose_cam2); - values.insert(body_P_cam1_key, Pose3::identity()); - values.insert(body_P_cam2_key, Pose3::identity()); + values.insert(body_P_cam1_key, Pose3::Identity()); + values.insert(body_P_cam2_key, Pose3::Identity()); SmartStereoProjectionFactorPP factor1(model); factor1.add(cam1_uv, x1, body_P_cam1_key, K2); @@ -426,7 +426,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_optimization_multipleExtrinsics ) { // Values Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0)); Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0.1, 1.0),Point3(1, 1, 1)); - Pose3 body_Pose_cam3 = Pose3::identity(); + Pose3 body_Pose_cam3 = Pose3::Identity(); Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse()); Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse()); Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse()); @@ -1147,7 +1147,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) { graph.push_back(smartFactor3); graph.addPrior(x1, pose1, noisePrior); graph.addPrior(x2, pose2, noisePrior); - graph.addPrior(body_P_cam_key, Pose3::identity(), noisePrior); + graph.addPrior(body_P_cam_key, Pose3::Identity(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1156,7 +1156,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) { values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3 * noise_pose); - values.insert(body_P_cam_key, Pose3::identity()); + values.insert(body_P_cam_key, Pose3::Identity()); // All smart factors are disabled and pose should remain where it is Values result; @@ -1245,7 +1245,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) { values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); - values.insert(body_P_cam_key, Pose3::identity()); + values.insert(body_P_cam_key, Pose3::Identity()); EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9); EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9); @@ -1267,7 +1267,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) { Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); - EXPECT(assert_equal(Pose3::identity(), result.at(body_P_cam_key))); + EXPECT(assert_equal(Pose3::Identity(), result.at(body_P_cam_key))); } /* ************************************************************************* */ diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 6d23144aab..5b27eea4db 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -382,7 +382,7 @@ TEST(ExpressionFactor, compose2) { TEST(ExpressionFactor, compose3) { // Create expression - Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R1(Rot3::Identity()), R2(3); Rot3_ R3 = R1 * R2; // Create factor diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index ac3a09e369..a05c4b621a 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -139,7 +139,7 @@ TEST(Manifold, DefaultChart) { Vector v3(3); v3 << 1, 1, 1; - Rot3 I = Rot3::identity(); + Rot3 I = Rot3::Identity(); Rot3 R = I.retract(v3); //DefaultChart chart5; EXPECT(assert_equal(v3, traits::Local(I, R))); diff --git a/timing/timeShonanFactor.cpp b/timing/timeShonanFactor.cpp index 207d54a4dc..60dd6f47a7 100644 --- a/timing/timeShonanFactor.cpp +++ b/timing/timeShonanFactor.cpp @@ -62,9 +62,9 @@ int main(int argc, char* argv[]) { // Build graph NonlinearFactorGraph graph; - // graph.add(NonlinearEquality(0, SOn::identity(4))); + // graph.add(NonlinearEquality(0, SOn::Identity(4))); auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); - graph.add(PriorFactor(0, SOn::identity(4), priorModel)); + graph.add(PriorFactor(0, SOn::Identity(4), priorModel)); auto G = boost::make_shared(SOn::VectorizedGenerators(4)); for (const auto &m : measurements) { const auto &keys = m.keys(); @@ -92,7 +92,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < 100; i++) { gttic_(optimize); Values initial; - initial.insert(0, SOn::identity(4)); + initial.insert(0, SOn::Identity(4)); for (size_t j = 1; j < poses.size(); j++) { initial.insert(j, SOn::Random(rng, 4)); }