From 76308a5d4616e3e6586d370799ce39aece9fd436 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 15 Apr 2016 16:54:46 -0400 Subject: [PATCH] Deprecated Vector zero(size_t n). All unit tests pass. --- gtsam/base/Vector.h | 5 ++- gtsam/base/numericalDerivative.h | 2 +- gtsam/base/tests/testVector.cpp | 11 +----- .../discrete/tests/testDiscreteMarginals.cpp | 2 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 6 ++-- gtsam/geometry/tests/testOrientedPlane3.cpp | 4 +-- gtsam/geometry/tests/testPose3.cpp | 8 ++--- gtsam/geometry/tests/testRot2.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 4 +-- gtsam/geometry/tests/testUnit3.cpp | 6 ++-- gtsam/linear/HessianFactor.cpp | 4 +-- gtsam/linear/JacobianFactor.cpp | 4 +-- gtsam/linear/NoiseModel.h | 2 +- gtsam/linear/RegularJacobianFactor.h | 4 +-- gtsam/linear/tests/testKalmanFilter.cpp | 2 +- gtsam/navigation/tests/testAttitudeFactor.cpp | 4 +-- gtsam/navigation/tests/testGPSFactor.cpp | 4 +-- gtsam/navigation/tests/testMagFactor.cpp | 8 ++--- gtsam/nonlinear/NonlinearEquality.h | 2 +- gtsam/nonlinear/NonlinearFactor.h | 12 +++---- .../tests/testLinearContainerFactor.cpp | 2 +- gtsam/slam/GeneralSFMFactor.h | 4 +-- gtsam/slam/SmartProjectionFactor.h | 4 +-- .../tests/testEssentialMatrixConstraint.cpp | 2 +- gtsam/slam/tests/testPoseRotationPrior.cpp | 4 +-- gtsam/slam/tests/testPoseTranslationPrior.cpp | 8 ++--- gtsam/slam/tests/testReferenceFrameFactor.cpp | 2 +- .../tests/testRegularImplicitSchurFactor.cpp | 2 +- gtsam/slam/tests/testRotateFactor.cpp | 4 +-- .../tests/testSmartProjectionCameraFactor.cpp | 2 +- .../tests/testSmartProjectionPoseFactor.cpp | 2 +- .../dynamics/tests/testPendulumFactors.cpp | 8 ++--- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 10 +++--- .../dynamics/tests/testSimpleHelicopter.cpp | 6 ++-- .../dynamics/tests/testVelocityConstraint.cpp | 12 +++---- .../tests/testVelocityConstraint3.cpp | 2 +- .../geometry/tests/testBearingS2.cpp | 8 ++--- .../geometry/tests/testPose3Upright.cpp | 10 +++--- .../geometry/tests/testSimilarity3.cpp | 2 +- gtsam_unstable/linear/QPSolver.cpp | 2 +- gtsam_unstable/linear/tests/testQPSolver.cpp | 28 +++++++-------- gtsam_unstable/nonlinear/LinearizedFactor.cpp | 4 +-- gtsam_unstable/slam/AHRS.cpp | 2 +- gtsam_unstable/slam/DummyFactor.cpp | 2 +- gtsam_unstable/slam/Mechanization_bRn2.h | 2 +- gtsam_unstable/slam/PartialPriorFactor.h | 2 +- gtsam_unstable/slam/SmartRangeFactor.h | 4 +-- .../slam/SmartStereoProjectionFactor.h | 4 +-- gtsam_unstable/slam/tests/testDummyFactor.cpp | 2 +- .../testInertialNavFactor_GlobalVelocity.cpp | 12 +++---- .../tests/testRelativeElevationFactor.cpp | 4 +-- .../testTransformBtwRobotsUnaryFactor.cpp | 4 +-- .../testTransformBtwRobotsUnaryFactorEM.cpp | 4 +-- tests/smallExample.h | 6 ++-- tests/testBoundingConstraint.cpp | 16 ++++----- tests/testExpressionFactor.cpp | 8 ++--- tests/testGaussianBayesTreeB.cpp | 34 +++++++++---------- tests/testGaussianFactorGraphB.cpp | 8 ++--- tests/testIterative.cpp | 6 ++-- tests/testManifold.cpp | 4 +-- tests/testNonlinearEquality.cpp | 16 ++++----- tests/testNonlinearOptimizer.cpp | 6 ++-- timing/timeSchurFactors.cpp | 2 +- 63 files changed, 181 insertions(+), 191 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 4627ff6bc..2d09dcc90 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -249,14 +249,13 @@ inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} inline double sum(const Vector &a){return a.sum();} +inline Vector zero(size_t n) { return Vector::Zero(n);} +#endif inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } -inline Vector zero(size_t n) { return Vector::Zero(n);} inline Vector ones(size_t n) { return Vector::Ones(n); } inline size_t dim(const Vector& v) { return v.size(); } -#endif - } // namespace gtsam #include diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 1bd62c257..ba72db820 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -88,7 +88,7 @@ typename internal::FixedSizeMatrix::type numericalGradient(boost::function::Retract(x, d)); diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 790e0922c..0fb80d7f0 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -86,15 +86,6 @@ TEST(Vector, zero1 ) EXPECT(zero(v)); } -/* ************************************************************************* */ -TEST(Vector, zero2 ) -{ - Vector a = zero(2); - Vector b = Vector::Zero(2); - EXPECT(a==b); - EXPECT(assert_equal(a, b)); -} - /* ************************************************************************* */ TEST(Vector, scalar_multiply ) { @@ -256,7 +247,7 @@ TEST(Vector, equals ) TEST(Vector, greater_than ) { Vector v1 = Vector3(1.0, 2.0, 3.0), - v2 = zero(3); + v2 = Z_3x1; EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than EXPECT(greaterThanOrEqual(v1, v2)); // test equals } diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 24bec4fa1..4e9f956b6 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -167,7 +167,7 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) { // Calculate the marginals by brute force vector allPosbValues = cartesianProduct( key[0] & key[1] & key[2] & key[3] & key[4]); - Vector T = zero(5), F = zero(5); + Vector T = Z_5x1, F = Z_5x1; for (size_t i = 0; i < allPosbValues.size(); ++i) { DiscreteFactor::Values x = allPosbValues[i]; double px = graph(x); diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 79759678d..75ee9427d 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -62,7 +62,7 @@ TEST (EssentialMatrix, FromPose3) { //******************************************************************************* TEST(EssentialMatrix, localCoordinates0) { EssentialMatrix E; - Vector expected = zero(5); + Vector expected = Z_5x1; Vector actual = E.localCoordinates(E); EXPECT(assert_equal(expected, actual, 1e-8)); } @@ -74,7 +74,7 @@ TEST (EssentialMatrix, localCoordinates) { Pose3 pose(trueRotation, trueTranslation); EssentialMatrix hx = EssentialMatrix::FromPose3(pose); Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); - EXPECT(assert_equal(zero(5), actual, 1e-8)); + EXPECT(assert_equal(Z_5x1, actual, 1e-8)); Vector6 d; d << 0.1, 0.2, 0.3, 0, 0, 0; @@ -85,7 +85,7 @@ TEST (EssentialMatrix, localCoordinates) { //************************************************************************* TEST (EssentialMatrix, retract0) { - EssentialMatrix actual = trueE.retract(zero(5)); + EssentialMatrix actual = trueE.retract(Z_5x1); EXPECT(assert_equal(trueE, actual)); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index d3a107570..7266aaf32 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -97,7 +97,7 @@ inline static Vector randomVector(const Vector& minLimits, // Get the number of dimensions and create the return vector size_t numDims = dim(minLimits); - Vector vector = zero(numDims); + Vector vector = Vector::Zero(numDims); // Create the random vector for (size_t i = 0; i < numDims; i++) { @@ -145,7 +145,7 @@ TEST (OrientedPlane3, error2) { OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); // Hard-coded regression values, to ensure the result doesn't change. - EXPECT(assert_equal(zero(3), plane1.errorVector(plane1), 1e-8)); + EXPECT(assert_equal((Vector) Z_3x1, plane1.errorVector(plane1), 1e-8)); EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5)); // Test the jacobians of transform diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 1b48c03a2..165870f25 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -61,7 +61,7 @@ TEST( Pose3, constructors) TEST( Pose3, retract_first_order) { Pose3 id; - Vector v = zero(6); + Vector v = Z_6x1; v(0) = 0.3; EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2)); v(3)=0.2;v(4)=0.7;v(5)=-2; @@ -71,7 +71,7 @@ TEST( Pose3, retract_first_order) /* ************************************************************************* */ TEST( Pose3, retract_expmap) { - Vector v = zero(6); v(0) = 0.3; + Vector v = Z_6x1; v(0) = 0.3; Pose3 pose = Pose3::Expmap(v); EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2)); EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); @@ -81,7 +81,7 @@ TEST( Pose3, retract_expmap) TEST( Pose3, expmap_a_full) { Pose3 id; - Vector v = zero(6); + Vector v = Z_6x1; v(0) = 0.3; EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; @@ -92,7 +92,7 @@ TEST( Pose3, expmap_a_full) TEST( Pose3, expmap_a_full2) { Pose3 id; - Vector v = zero(6); + Vector v = Z_6x1; v(0) = 0.3; EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 4394103c9..d6d1f3149 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -89,7 +89,7 @@ TEST( Rot2, equals) /* ************************************************************************* */ TEST( Rot2, expmap) { - Vector v = zero(1); + Vector v = Z_1x1; CHECK(assert_equal(R.retract(v), R)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 0c78cc426..5e72d4c5b 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -152,7 +152,7 @@ TEST( Rot3, Rodrigues4) /* ************************************************************************* */ TEST( Rot3, retract) { - Vector v = zero(3); + Vector v = Z_3x1; CHECK(assert_equal(R, R.retract(v))); // // test Canonical coordinates @@ -213,7 +213,7 @@ TEST(Rot3, log) #define CHECK_OMEGA_ZERO(X,Y,Z) \ w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ R = Rot3::Rodrigues(w); \ - EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); + EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R))); CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 396c8b0f3..0e99268ee 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -237,7 +237,7 @@ TEST(Unit3, distance) { TEST(Unit3, localCoordinates0) { Unit3 p; Vector actual = p.localCoordinates(p); - EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal(Z_2x1, actual, 1e-8)); } TEST(Unit3, localCoordinates) { @@ -245,14 +245,14 @@ TEST(Unit3, localCoordinates) { Unit3 p, q; Vector2 expected = Vector2::Zero(); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-8)); } { Unit3 p, q(1, 6.12385e-21, 0); Vector2 expected = Vector2::Zero(); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-8)); } { diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9277f3903..1c55d293b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -377,7 +377,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, vector y; y.reserve(size()); for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); + y.push_back(Vector::Zero(getDim(it))); // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column @@ -427,7 +427,7 @@ void HessianFactor::gradientAtZero(double* d) const { Vector HessianFactor::gradient(Key key, const VectorValues& x) const { Factor::const_iterator i = find(key); // Sum over G_ij*xj for all xj connecting to xi - Vector b = zero(x.at(key).size()); + Vector b = Vector::Zero(x.at(key).size()); for (Factor::const_iterator j = begin(); j != end(); ++j) { // Obtain Gij from the Hessian factor // Hessian factor only stores an upper triangular matrix, so be careful when i>j diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 4bc141798..7fecefe3c 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -543,7 +543,7 @@ void JacobianFactor::updateHessian(const FastVector& infoKeys, /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); if (empty()) return Ax; @@ -594,7 +594,7 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y if (empty()) return; - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); /// Just iterate over all A matrices and multiply in correct config part (looping over keys) /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index d0c7a58db..3a8a6744c 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -381,7 +381,7 @@ namespace gtsam { * from appearing in invsigmas or precisions. * mu set to large default value (1000.0) */ - Constrained(const Vector& sigmas = zero(1)); + Constrained(const Vector& sigmas = Z_1x1); /** * Constructor that prevents any inf values diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h index f954cba88..bbc34d482 100644 --- a/gtsam/linear/RegularJacobianFactor.h +++ b/gtsam/linear/RegularJacobianFactor.h @@ -83,7 +83,7 @@ public: void multiplyHessianAdd(double alpha, const double* x, double* y) const { if (empty()) return; - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); // Just iterate over all A matrices and multiply in correct config part for (size_t pos = 0; pos < size(); ++pos) @@ -173,7 +173,7 @@ public: * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way */ Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); if (empty()) return Ax; diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index b095c5a31..1e676b977 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -198,7 +198,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished(); Matrix B = Matrix::Zero(9, 1); - Vector u = zero(1); + Vector u = Z_1x1; Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) << 33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 790080556..f7f0aa9ad 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -42,7 +42,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { // Create a linearization point at the zero-error point Rot3 nRb; - EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); + EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( @@ -75,7 +75,7 @@ TEST( Pose3AttitudeFactor, Constructor ) { // Create a linearization point at the zero-error point Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); - EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); + EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index de318b3e4..d9ba38e02 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -58,7 +58,7 @@ TEST( GPSFactor, Constructor ) { // Create a linearization point at zero error Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); - EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); + EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( @@ -87,7 +87,7 @@ TEST( GPSFactor2, Constructor ) { // Create a linearization point at zero error NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero()); - EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); + EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 38aecfcbc..1cc84751c 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -71,19 +71,19 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); - EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); + EXPECT( assert_equal(Z_3x3,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); - EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5)); + EXPECT( assert_equal(Z_3x3,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); - EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); + EXPECT( assert_equal(Z_3x3,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); @@ -93,7 +93,7 @@ TEST( MagFactor, Factors ) { // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); - EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); + EXPECT(assert_equal(Z_3x3,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index ccdcb86be..0d8d717bc 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -147,7 +147,7 @@ public: } else if (compare_(feasible_, xj)) { if (H) *H = Matrix::Identity(nj,nj); - return zero(nj); // set error to zero if equal + return Vector::Zero(nj); // set error to zero if equal } else { if (H) throw std::invalid_argument( diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ec77b2bd6..505f58394 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -313,7 +313,7 @@ public: return evaluateError(x1); } } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -388,7 +388,7 @@ public: return evaluateError(x1, x2); } } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -463,7 +463,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -543,7 +543,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -627,7 +627,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -715,7 +715,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 7dd4a1f8b..df1df0d20 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -234,7 +234,7 @@ TEST( testLinearContainerFactor, creation ) { // create a linear factor SharedDiagonal model = noiseModel::Unit::Create(2); JacobianFactor::shared_ptr linear_factor(new JacobianFactor( - l3, I_2x2, l5, 2.0 * I_2x2, zero(2), model)); + l3, I_2x2, l5, 2.0 * I_2x2, Z_2x1, model)); // create a set of values - build with full set of values gtsam::Values full_values, exp_values; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index db7dc4e74..0f187db75 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -129,7 +129,7 @@ public: if (H1) *H1 = JacobianC::Zero(); if (H2) *H2 = JacobianL::Zero(); // TODO warn if verbose output asked for - return zero(2); + return Z_2x1; } } @@ -272,7 +272,7 @@ public: std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; } - return zero(2); + return Z_2x1; } /** return the measured */ diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 9d0f9c554..01f89e526 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -293,7 +293,7 @@ public: BOOST_FOREACH(Matrix& m, Gs) m = Matrix::Zero(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(Base::Dim); + v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, Gs, gs, 0.0); } @@ -477,7 +477,7 @@ public: if (nonDegenerate) return Base::unwhitenedError(cameras, *result_); else - return zero(cameras.size() * 2); + return Vector::Zero(cameras.size() * 2); } /** diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 31b276642..b2150dd86 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -45,7 +45,7 @@ TEST( EssentialMatrixConstraint, test ) { Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840), Point3(-3.37493895, 6.14660244, -8.93650986)); - Vector expected = zero(5); + Vector expected = Z_5x1; Vector actual = factor.evaluateError(pose1,pose2); CHECK(assert_equal(expected, actual, 1e-8)); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 3c7d5f2b2..f61beab6c 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -52,7 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) { Pose3 pose1(rot3A, point3A); Pose3RotationPrior factor(poseKey, rot3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -78,7 +78,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) { Pose2 pose1(rot2A, point2A); Pose2RotationPrior factor(poseKey, rot2A, model1); Matrix actH1; - EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 2fd471c9c..bbc0c6518 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -48,7 +48,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -68,7 +68,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -88,7 +88,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -108,7 +108,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2A, model2); Matrix actH1; - EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_2x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 309801ccb..f955aa191 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -93,7 +93,7 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { PointReferenceFrameFactor tc(lA1, tA1, lB1); Vector actCost = tc.evaluateError(global, trans, local), - expCost = zero(2); + expCost = Z_2x1; EXPECT(assert_equal(expCost, actCost, 1e-5)); Matrix actualDT, actualDL, actualDF; diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 261cc1716..4df3ed1e6 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -89,7 +89,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { FastVector keys2; keys2 += 0,1,2,3; Vector x = xvalues.vector(keys2); - Vector expected = zero(24); + Vector expected = Vector::Zero(24); RegularImplicitSchurFactor::multiplyHessianAdd(F, E, P, alpha, x, expected); EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8)); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index a8f48b050..9bb296444 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -58,7 +58,7 @@ TEST (RotateFactor, checkMath) { TEST (RotateFactor, test) { Model model = noiseModel::Isotropic::Sigma(3, 0.01); RotateFactor f(1, i1Ri2, c1Zc2, model); - EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); + EXPECT(assert_equal(Z_3x1, f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -127,7 +127,7 @@ TEST (RotateFactor, minimization) { TEST (RotateDirectionsFactor, test) { Model model = noiseModel::Isotropic::Sigma(2, 0.01); RotateDirectionsFactor f(1, p1, z1, model); - EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); + EXPECT(assert_equal(Z_2x1, f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index daecb56bf..7eefb2398 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -123,7 +123,7 @@ TEST( SmartProjectionCameraFactor, noiseless ) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); CHECK( - assert_equal(zero(4), + assert_equal(Z_4x1, factor1->reprojectionErrorAfterTriangulation(values), 1e-7)); } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index b710c252d..ace75f80f 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -140,7 +140,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT(assert_equal(zero(4), actualErrors, 1e-7)); + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); // Calculate using computeJacobians Vector b; diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp index 5a4593270..d4b877008 100644 --- a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp +++ b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp @@ -29,7 +29,7 @@ TEST( testPendulumFactor1, evaluateError) { PendulumFactor1 constraint(Q(2), Q(1), V(1), h); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(q2, q1, v1), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(q2, q1, v1), tol)); } /* ************************************************************************* */ @@ -38,7 +38,7 @@ TEST( testPendulumFactor2, evaluateError) { PendulumFactor2 constraint(V(2), V(1), Q(1), h); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(v2, v1, q1), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(v2, v1, q1), tol)); } /* ************************************************************************* */ @@ -49,7 +49,7 @@ TEST( testPendulumFactorPk, evaluateError) { double pk( 1/h * (q2-q1) + h*g*sin(q1) ); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk, q1, q2), tol)); } /* ************************************************************************* */ @@ -60,7 +60,7 @@ TEST( testPendulumFactorPk1, evaluateError) { double pk1( 1/h * (q2-q1) ); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk1, q1, q2), tol)); } diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 1e52988c0..025c838c9 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -76,12 +76,12 @@ TEST( testPoseRTV, equals ) { TEST( testPoseRTV, Lie ) { // origin and zero deltas PoseRTV identity; - EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)))); - EXPECT(assert_equal(zero(9), identity.localCoordinates(identity))); + EXPECT(assert_equal(identity, (PoseRTV)identity.retract(Z_9x1))); + EXPECT(assert_equal((Vector) Z_9x1, identity.localCoordinates(identity))); PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)))); - EXPECT(assert_equal(zero(9), state1.localCoordinates(state1))); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(Z_9x1))); + EXPECT(assert_equal((Vector) Z_9x1, state1.localCoordinates(state1))); Vector delta(9); delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3; @@ -111,7 +111,7 @@ TEST( testPoseRTV, dynamics_identities ) { const double dt = 0.1; Vector accel = Vector3(0.2, 0.0, 0.0), gyro = Vector3(0.0, 0.0, 0.2); - Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6); + Vector imu01 = Z_6x1, imu12 = Z_6x1, imu23 = Z_6x1, imu34 = Z_6x1; x1 = x0.generalDynamics(accel, gyro, dt); x2 = x1.generalDynamics(accel, gyro, dt); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index db5c2bc6e..fe21d5e00 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -53,7 +53,7 @@ TEST( Reconstruction, evaluateError) { // verify error function Matrix H1, H2, H3; EXPECT( - assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); + assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( boost::function( @@ -89,7 +89,7 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Vector Fu = computeFu(gamma2, u2); - Vector fGravity_g1 = zero(6); + Vector fGravity_g1 = Z_6x1; fGravity_g1.segment<3>(3) = g1.rotation().unrotate(Vector3(0, 0, -mass*9.81)); // gravity force in g1 frame Vector Fb = Fu+fGravity_g1; @@ -106,7 +106,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { // verify error function Matrix H1, H2, H3; - EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); + EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( boost::function( diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index f253be371..429edb760 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -25,8 +25,8 @@ TEST( testVelocityConstraint, trapezoidal ) { VelocityConstraint constraint(x1, x2, dynamics::TRAPEZOIDAL, dt); // verify error function - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, pose1), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); EXPECT(assert_equal(delta(3, 0,-1.0), constraint.evaluateError(pose1, pose1), tol)); EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); } @@ -38,8 +38,8 @@ TEST( testEulerVelocityConstraint, euler_start ) { // verify error function EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol)); EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); } @@ -50,8 +50,8 @@ TEST( testEulerVelocityConstraint, euler_end ) { // verify error function EXPECT(assert_equal(delta(3, 0,-0.5), constraint.evaluateError(origin, pose1), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol)); EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); } diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp index ac27ae563..b8caf4414 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp @@ -23,7 +23,7 @@ TEST( testVelocityConstraint3, evaluateError) { VelocityConstraint3 constraint(X(1), X(2), V(1), dt); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(x1, x2, v), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testBearingS2.cpp b/gtsam_unstable/geometry/tests/testBearingS2.cpp index 48d6e29af..f29b30621 100644 --- a/gtsam_unstable/geometry/tests/testBearingS2.cpp +++ b/gtsam_unstable/geometry/tests/testBearingS2.cpp @@ -48,11 +48,11 @@ TEST( testBearingS2, manifold ) { BearingS2 origin, b1(0.2, 0.3); EXPECT_LONGS_EQUAL(2, origin.dim()); - EXPECT(assert_equal(zero(2), origin.localCoordinates(origin), tol)); - EXPECT(assert_equal(origin, origin.retract(zero(2)), tol)); + EXPECT(assert_equal(Z_2x1, origin.localCoordinates(origin), tol)); + EXPECT(assert_equal(origin, origin.retract(Z_2x1), tol)); - EXPECT(assert_equal(zero(2), b1.localCoordinates(b1), tol)); - EXPECT(assert_equal(b1, b1.retract(zero(2)), tol)); + EXPECT(assert_equal(Z_2x1, b1.localCoordinates(b1), tol)); + EXPECT(assert_equal(b1, b1.retract(Z_2x1), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index cd54a8813..5e4c2468d 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -68,9 +68,9 @@ TEST( testPose3Upright, manifold ) { Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.0), x2(4.0, 2.0, 7.0, 0.0); EXPECT_LONGS_EQUAL(4, origin.dim()); - EXPECT(assert_equal(origin, origin.retract(zero(4)), tol)); - EXPECT(assert_equal(x1, x1.retract(zero(4)), tol)); - EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); + EXPECT(assert_equal(origin, origin.retract(Z_4x1), tol)); + EXPECT(assert_equal(x1, x1.retract(Z_4x1), tol)); + EXPECT(assert_equal(x2, x2.retract(Z_4x1), tol)); Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0).finished(), delta21 = -delta12; EXPECT(assert_equal(x2, x1.retract(delta12), tol)); @@ -83,8 +83,8 @@ TEST( testPose3Upright, manifold ) { /* ************************************************************************* */ TEST( testPose3Upright, lie ) { Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.1); - EXPECT(assert_equal(zero(4), Pose3Upright::Logmap(origin), tol)); - EXPECT(assert_equal(origin, Pose3Upright::Expmap(zero(4)), tol)); + EXPECT(assert_equal(Z_4x1, Pose3Upright::Logmap(origin), tol)); + EXPECT(assert_equal(origin, Pose3Upright::Expmap(Z_4x1), tol)); EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol)); } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 1866fafc6..d2fbd7579 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -156,7 +156,7 @@ TEST(Similarity3, Manifold) { //****************************************************************************** TEST( Similarity3, retract_first_order) { Similarity3 id; - Vector v = zero(7); + Vector v = Z_7x1; v(0) = 0.3; EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); // v(3) = 0.2; diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index f0eb7d7fb..0d9bbae6e 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -51,7 +51,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, // Collect the gradients of unconstrained cost factors to the b vector if (Aterms.size() > 0) { - Vector b = zero(delta.at(key).size()); + Vector b = Vector::Zero(delta.at(key).size()); if (costVariableIndex_.find(key) != costVariableIndex_.end()) { BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index d643223f8..ebe927738 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -39,7 +39,7 @@ QP createTestCase() { // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 qp.cost.push_back( HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * ones(1), - 2.0 * Matrix::Ones(1, 1), zero(1), 10.0)); + 2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), X(2), Matrix::Ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 @@ -92,8 +92,8 @@ QP createEqualityConstrainedTest() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, zero(1), - 2.0 * Matrix::Ones(1, 1), zero(1), 0.0)); + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, Z_1x1, + 2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector @@ -129,8 +129,8 @@ TEST(QPSolver, indentifyActiveConstraints) { QPSolver solver(qp); VectorValues currentSolution; - currentSolution.insert(X(1), zero(1)); - currentSolution.insert(X(2), zero(1)); + currentSolution.insert(X(1), Z_1x1); + currentSolution.insert(X(2), Z_1x1); LinearInequalityFactorGraph workingSet = solver.identifyActiveConstraints(qp.inequalities, currentSolution); @@ -154,8 +154,8 @@ TEST(QPSolver, iterate) { QPSolver solver(qp); VectorValues currentSolution; - currentSolution.insert(X(1), zero(1)); - currentSolution.insert(X(2), zero(1)); + currentSolution.insert(X(1), Z_1x1); + currentSolution.insert(X(2), Z_1x1); std::vector expectedSolutions(4), expectedDuals(4); expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished()); @@ -199,8 +199,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { QP qp = createTestCase(); QPSolver solver(qp); VectorValues initialValues; - initialValues.insert(X(1), zero(1)); - initialValues.insert(X(2), zero(1)); + initialValues.insert(X(1), Z_1x1); + initialValues.insert(X(2), Z_1x1); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); VectorValues expectedSolution; @@ -236,8 +236,8 @@ TEST(QPSolver, optimizeMatlabEx) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); VectorValues initialValues; - initialValues.insert(X(1), zero(1)); - initialValues.insert(X(2), zero(1)); + initialValues.insert(X(1), Z_1x1); + initialValues.insert(X(2), Z_1x1); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); VectorValues expectedSolution; @@ -269,7 +269,7 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), (Vector(1) << 2.0).finished()); - initialValues.insert(X(2), zero(1)); + initialValues.insert(X(2), Z_1x1); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); @@ -283,8 +283,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { TEST(QPSolver, failedSubproblem) { QP qp; - qp.cost.push_back(JacobianFactor(X(1), I_2x2, zero(2))); - qp.cost.push_back(HessianFactor(X(1), Z_2x2, zero(2), 100.0)); + qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); + qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0)); qp.inequalities.push_back( LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index f8dca75a4..84dffe7be 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -194,7 +194,7 @@ bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol double LinearizedHessianFactor::error(const Values& c) const { // Construct an error vector in key-order from the Values - Vector dx = zero(dim()); + Vector dx = Vector::Zero(dim()); size_t index = 0; for(unsigned int i = 0; i < this->size(); ++i){ Key key = this->keys()[i]; @@ -217,7 +217,7 @@ boost::shared_ptr LinearizedHessianFactor::linearize(const Values& c) const { // Construct an error vector in key-order from the Values - Vector dx = zero(dim()); + Vector dx = Vector::Zero(dim()); size_t index = 0; for(unsigned int i = 0; i < this->size(); ++i){ Key key = this->keys()[i]; diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index a16a4f9c0..a0beef07f 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -101,7 +101,7 @@ std::pair AHRS::initialize(double g_e) P_plus_k2.block<3,3>(6, 3) = Z_3x3; P_plus_k2.block<3,3>(6, 6) = Pa; - Vector dx = zero(9); + Vector dx = Z_9x1; KalmanFilter::State state = KF_.init(dx, P_plus_k2); return std::make_pair(mech0_, state); } diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 70aff0596..8519f6f8d 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -55,7 +55,7 @@ DummyFactor::linearize(const Values& c) const { noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_); return GaussianFactor::shared_ptr( - new JacobianFactor(terms, zero(rowDim_), model)); + new JacobianFactor(terms, Vector::Zero(rowDim_), model)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 4268aa0e5..a228b2347 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -30,7 +30,7 @@ public: * @param r3 Z-axis of rotated frame */ Mechanization_bRn2(const Rot3& initial_bRn = Rot3(), - const Vector3& initial_x_g = zero(3), const Vector3& initial_x_a = zero(3)) : + const Vector3& initial_x_g = Z_3x1, const Vector3& initial_x_a = Z_3x1) : bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) { } diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index a848620e3..fa06d47a3 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -113,7 +113,7 @@ namespace gtsam { // FIXME: this was originally the generic retraction - may not produce same results Vector full_logmap = T::Logmap(p); // Vector full_logmap = T::identity().localCoordinates(p); // Alternate implementation - Vector masked_logmap = zero(this->dim()); + Vector masked_logmap = Vector::Zero(this->dim()); for (size_t i=0; i >(this->keys_, Gs, gs, 0.0); } @@ -528,7 +528,7 @@ public: if (nonDegenerate) return Base::unwhitenedError(cameras, *result_); else - return zero(cameras.size() * Base::ZDim); + return Vector::Zero(cameras.size() * Base::ZDim); } /** diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp index 03a57b352..743c7398c 100644 --- a/gtsam_unstable/slam/tests/testDummyFactor.cpp +++ b/gtsam_unstable/slam/tests/testDummyFactor.cpp @@ -47,7 +47,7 @@ TEST( testDummyFactor, basics ) { CHECK(actLinearization); noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); GaussianFactor::shared_ptr expLinearization(new JacobianFactor( - key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), zero(3), model3)); + key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), Z_3x1, model3)); EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); } diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 75535eebc..b84f7e080 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -152,7 +152,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -185,7 +185,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -225,7 +225,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); // TODO: Expected values need to be updated for global velocity version CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); @@ -488,7 +488,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -529,7 +529,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -579,7 +579,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); // TODO: Expected values need to be updated for global velocity version CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index a06c39182..210018ed3 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -34,7 +34,7 @@ TEST( testRelativeElevationFactor, level_zero_error ) { double measured = 2.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2))); + EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -79,7 +79,7 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) { double measured = 2.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2))); + EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index d8d720e83..01d4b152d 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -97,7 +97,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, (Vector) Z_3x1, 1e-5)); } /* ************************************************************************* */ @@ -131,7 +131,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, Z_3x1, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index dbd90f3a3..b200a3e58 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -108,7 +108,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, Z_3x1, 1e-5)); } /* ************************************************************************* */ @@ -147,7 +147,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, Z_3x1, 1e-5)); } /* ************************************************************************* */ diff --git a/tests/smallExample.h b/tests/smallExample.h index b2a2102e1..d46cc34e1 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -260,9 +260,9 @@ inline VectorValues createZeroDelta() { using symbol_shorthand::X; using symbol_shorthand::L; VectorValues c; - c.insert(L(1), zero(2)); - c.insert(X(1), zero(2)); - c.insert(X(2), zero(2)); + c.insert(L(1), Z_2x1); + c.insert(X(1), Z_2x1); + c.insert(X(2), Z_2x1); return c; } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index d4cefd329..18dabf18b 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -56,8 +56,8 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) { EXPECT(constraint2.isGreaterThan()); EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol)); EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol)); - EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol); EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol); } @@ -75,8 +75,8 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) { EXPECT(!constraint4.isGreaterThan()); EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint3.evaluateError(pt2), tol)); EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint4.evaluateError(pt2), tol)); - EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol); EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol); } @@ -189,26 +189,26 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1))); EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); - EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); + EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3))); EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); Values config1; config1.insert(key1, pt1); config1.insert(key2, pt1); EXPECT(!rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1))); EXPECT(!rangeBound.linearize(config1)); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt2); EXPECT(!rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1))); EXPECT(!rangeBound.linearize(config1)); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt3); EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt4); diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index c20eca34a..4fda27cdb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -338,7 +338,7 @@ TEST(ExpressionFactor, Compose1) { EXPECT( assert_equal(I_3x3, H[1],1e-9)); // Check linearization - JacobianFactor expected(1, I_3x3, 2, I_3x3, zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -367,7 +367,7 @@ TEST(ExpressionFactor, compose2) { EXPECT( assert_equal(2*I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(1, 2 * I_3x3, zero(3)); + JacobianFactor expected(1, 2 * I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -396,7 +396,7 @@ TEST(ExpressionFactor, compose3) { EXPECT( assert_equal(I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(3, I_3x3, zero(3)); + JacobianFactor expected(3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -441,7 +441,7 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(I_3x3, H[2],1e-9)); // Check linearization - JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index 9b172d572..e3c17fa3f 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -79,7 +79,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma3 = 0.61808; Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished(); GaussianBayesNet expected3; - expected3 += GaussianConditional(X(5), zero(2), I_2x2/sigma3, X(6), A56/sigma3); + expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3); GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; GaussianBayesNet actual3 = C3->shortcut(R); EXPECT(assert_equal(expected3,actual3,tol)); @@ -88,7 +88,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma4 = 0.661968; Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished(); GaussianBayesNet expected4; - expected4 += GaussianConditional(X(4), zero(2), I_2x2/sigma4, X(6), A46/sigma4); + expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4); GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; GaussianBayesNet actual4 = C4->shortcut(R); EXPECT(assert_equal(expected4,actual4,tol)); @@ -132,7 +132,7 @@ TEST( GaussianBayesTree, balanced_smoother_marginals ) double tol=1e-5; // Check marginal on x1 - JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1); + JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1); JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1); Matrix actualCovarianceX1; @@ -143,22 +143,22 @@ TEST( GaussianBayesTree, balanced_smoother_marginals ) // Check marginal on x2 double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), zero(2), sigx2); + JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2); JacobianFactor actual2 = *bayesTree.marginalFactor(X(2)); EXPECT(assert_equal(expected2,actual2,tol)); // Check marginal on x3 - JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), zero(2), sigmax3); + JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3); JacobianFactor actual3 = *bayesTree.marginalFactor(X(3)); EXPECT(assert_equal(expected3,actual3,tol)); // Check marginal on x4 - JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), zero(2), sigmax4); + JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4); JacobianFactor actual4 = *bayesTree.marginalFactor(X(4)); EXPECT(assert_equal(expected4,actual4,tol)); // Check marginal on x7 (should be equal to x1) - JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), zero(2), sigmax7); + JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7); JacobianFactor actual7 = *bayesTree.marginalFactor(X(7)); EXPECT(assert_equal(expected7,actual7,tol)); } @@ -212,8 +212,8 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts ) // // // Check the clique marginal P(C3) // double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED! -// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt); -// push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2)); +// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],Z_2x1,sigmax2_alt); +// push_front(expected,ordering[X(1)], Z_2x1, eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2)); // GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]]; // GaussianFactorGraph marginal = C3->marginal(R); // GaussianVariableIndex varIndex(marginal); @@ -248,17 +248,17 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) GaussianBayesNet expected1 = list_of // Why does the sign get flipped on the prior? - (GaussianConditional(X(1), zero(2), I/sigmax7, X(7), A/sigmax7)) - (GaussianConditional(X(7), zero(2), -1*I/sigmax7)); + (GaussianConditional(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7)) + (GaussianConditional(X(7), Z_2x1, -1*I/sigmax7)); GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7)); EXPECT(assert_equal(expected1, actual1, tol)); // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // GaussianBayesNet expected2; // GaussianConditional::shared_ptr - // parent2(new GaussianConditional(X(1), zero(2), -1*I/sigmax1, ones(2))); + // parent2(new GaussianConditional(X(1), Z_2x1, -1*I/sigmax1, ones(2))); // expected2.push_front(parent2); - // push_front(expected2,X(7), zero(2), I/sigmax1, X(1), A/sigmax1, sigma); + // push_front(expected2,X(7), Z_2x1, I/sigmax1, X(1), A/sigmax1, sigma); // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1)); // EXPECT(assert_equal(expected2,actual2,tol)); @@ -266,19 +266,19 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) double sig14 = 0.784465; Matrix A14 = -0.0769231*I; GaussianBayesNet expected3 = list_of - (GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14)) - (GaussianConditional(X(4), zero(2), I/sigmax4)); + (GaussianConditional(X(1), Z_2x1, I/sig14, X(4), A14/sig14)) + (GaussianConditional(X(4), Z_2x1, I/sigmax4)); GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4)); EXPECT(assert_equal(expected3,actual3,tol)); // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // GaussianBayesNet expected4; // GaussianConditional::shared_ptr - // parent4(new GaussianConditional(X(1), zero(2), -1.0*I/sigmax1, ones(2))); + // parent4(new GaussianConditional(X(1), Z_2x1, -1.0*I/sigmax1, ones(2))); // expected4.push_front(parent4); // double sig41 = 0.668096; // Matrix A41 = -0.055794*I; - // push_front(expected4,X(4), zero(2), I/sig41, X(1), A41/sig41, sigma); + // push_front(expected4,X(4), Z_2x1, I/sig41, X(1), A41/sig41, sigma); // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1)); // EXPECT(assert_equal(expected4,actual4,tol)); } diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 47307d0f8..becd407aa 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace) SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord[X(1)], I_3x3, ord[X(2)], I_3x3, zero(3), noise)); + ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord[X(2)], I_3x3, ord[X(3)], I_3x3, zero(3), noise)); + ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord[X(3)], I_3x3, ord[X(4)], I_3x3, zero(3), noise)); + ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord[X(5)], I_3x3, ord[X(6)], I_3x3, zero(3), noise)); + ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x1, noise)); GaussianFactorGraph actual; actual.push_back(f1); diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 071b9d12d..e0c2b7b66 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -62,7 +62,7 @@ TEST( Iterative, conjugateGradientDescent ) // get matrices Matrix A; Vector b; - Vector x0 = gtsam::zero(6); + Vector x0 = Z_6x1; boost::tie(A, b) = fg.jacobian(); Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2).finished(); @@ -104,7 +104,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(X(1), zero(3)); + expected.insert(X(1), Z_3x1); expected.insert(X(2), Vector3(-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } @@ -131,7 +131,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(X(1), zero(3)); + expected.insert(X(1), Z_3x1); expected.insert(X(2), Vector3(-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 89b296824..65d26eb98 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -131,7 +131,7 @@ TEST(Manifold, DefaultChart) { EXPECT_DOUBLES_EQUAL(traits::Retract(0, v1), 1, 1e-9); // Dynamic does not work yet ! - Vector z = zero(2), v(2); + Vector z = Z_2x1, v(2); v << 1, 0; //DefaultChart chart4; // EXPECT(assert_equal(traits::Local(z, v), v)); @@ -146,7 +146,7 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(traits::Retract(I, v3), R)); // Check zero vector //DefaultChart chart6; - EXPECT(assert_equal(zero(3), traits::Local(R, R))); + EXPECT(assert_equal((Vector) Z_3x1, traits::Local(R, R))); } //****************************************************************************** diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 335b68662..86080b673 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactor expLF(key, I_3x3, zero(3), constraintModel); + JacobianFactor expLF(key, I_3x3, Z_3x1, constraintModel); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } @@ -133,7 +133,7 @@ TEST ( NonlinearEquality, error ) { // check error function outputs Vector actual = nle->unwhitenedError(feasible); - EXPECT(assert_equal(actual, zero(3))); + EXPECT(assert_equal(actual, Z_3x1)); actual = nle->unwhitenedError(bad_linearize); EXPECT( @@ -263,8 +263,8 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { Values config1; config1.insert(key, pt); EXPECT(constraint.active(config1)); - EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol)); - EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT(assert_equal(Z_2x1, constraint.evaluateError(pt), tol)); + EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); Values config2; @@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key, I_2x2, zero(2), hard_model)); + new JacobianFactor(key, I_2x2, Z_2x1, hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; @@ -345,8 +345,8 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { config1.insert(key1, x1); config1.insert(key2, x2); EXPECT(constraint.active(config1)); - EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol)); - EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT(assert_equal(Z_2x1, constraint.evaluateError(x1, x2), tol)); + EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); Values config2; @@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key1, -I_2x2, key2, I_2x2, zero(2), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, Z_2x1, hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index f6cdd6ee5..7a22abc67 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -257,9 +257,9 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { expected.insert(2, Pose2(1, 1, M_PI)); VectorValues expectedGradient; - expectedGradient.insert(0,zero(3)); - expectedGradient.insert(1,zero(3)); - expectedGradient.insert(2,zero(3)); + expectedGradient.insert(0,Z_3x1); + expectedGradient.insert(1,Z_3x1); + expectedGradient.insert(2,Z_3x1); // Try LM and Dogleg LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults(); diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index 29e8e9916..9be55f831 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -109,7 +109,7 @@ void timeAll(size_t m, size_t N) { double* xdata = x.data(); // create a y - Vector y = zero(m * D); + Vector y = Vector::Zero(m * D); TIME(RawImplicit, implicitFactor, xdata, y.data()) TIME(RawJacobianQ, jf, xdata, y.data()) TIME(RawJacobianQR, jqr, xdata, y.data())