diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index c69a0ad20..dbfe7ab87 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -42,11 +42,6 @@ bool zero(const Vector& v) { return result; } -/* ************************************************************************* */ -Vector repeat(size_t n, double value) { - return Vector::Constant(n, value); -} - /* ************************************************************************* */ Vector delta(size_t n, size_t i, double value) { return Vector::Unit(n, i) * value; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index d4502b6ad..9438937ce 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -63,13 +63,6 @@ GTSAM_MAKE_VECTOR_DEFS(12); typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; -/** - * Create vector initialized to a constant value - * @param n is the size of the vector - * @param value is a constant value to insert into the vector - */ -GTSAM_EXPORT Vector repeat(size_t n, double value); - /** * Create basis vector of dimension n, * with a constant in spot i @@ -281,14 +274,15 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();} -GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} +GTSAM_EXPORT inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} -GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} -GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} +GTSAM_EXPORT inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);} +GTSAM_EXPORT inline const SubVector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} +GTSAM_EXPORT inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} #endif diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 0e7da537a..e096f23ce 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -538,7 +538,7 @@ TEST(Pose3, retract_localCoordinates) /* ************************************************************************* */ TEST(Pose3, expmap_logmap) { - Vector d12 = repeat(6,0.1); + Vector d12 = Vector::Constant(6,0.1); Pose3 t1 = T, t2 = t1.expmap(d12); EXPECT(assert_equal(d12, t1.logmap(t2))); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 9621898bf..f5e0a6913 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -224,14 +224,14 @@ TEST(Rot3, log) /* ************************************************************************* */ TEST(Rot3, retract_localCoordinates) { - Vector3 d12 = repeat(3,0.1); + Vector3 d12 = Vector::Constant(3,0.1); Rot3 R2 = R.retract(d12); EXPECT(assert_equal(d12, R.localCoordinates(R2))); } /* ************************************************************************* */ TEST(Rot3, expmap_logmap) { - Vector3 d12 = repeat(3,0.1); + Vector3 d12 = Vector::Constant(3,0.1); Rot3 R2 = R.expmap(d12); EXPECT(assert_equal(d12, R.logmap(R2))); } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 5212374a7..31b059989 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -326,7 +326,7 @@ static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { /* ************************************************************************* */ Constrained::Constrained(const Vector& sigmas) - : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) { + : Diagonal(sigmas), mu_(Vector::Constant(sigmas.size(), 1000.0)) { internal::fix(sigmas, precisions_, invsigmas_); } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index cef270100..c1718c0b3 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -416,7 +416,7 @@ namespace gtsam { * standard devations, some of which might be zero */ static shared_ptr MixedSigmas(const Vector& sigmas) { - return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas); + return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas); } /** @@ -424,7 +424,7 @@ namespace gtsam { * standard devations, some of which might be zero */ static shared_ptr MixedSigmas(double m, const Vector& sigmas) { - return MixedSigmas(repeat(sigmas.size(), m), sigmas); + return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas); } /** @@ -458,17 +458,17 @@ namespace gtsam { /** Fully constrained variations */ static shared_ptr All(size_t dim) { - return shared_ptr(new Constrained(repeat(dim, 1000.0), repeat(dim,0))); + return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0))); } /** Fully constrained variations */ static shared_ptr All(size_t dim, const Vector& mu) { - return shared_ptr(new Constrained(mu, repeat(dim,0))); + return shared_ptr(new Constrained(mu, Vector::Constant(dim,0))); } /** Fully constrained variations with a mu parameter */ static shared_ptr All(size_t dim, double mu) { - return shared_ptr(new Constrained(repeat(dim, mu), repeat(dim,0))); + return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0))); } virtual void print(const std::string& name) const; @@ -522,10 +522,10 @@ namespace gtsam { /** protected constructor takes sigma */ Isotropic(size_t dim, double sigma) : - Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} + Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} /* dummy constructor to allow for serialization */ - Isotropic() : Diagonal(repeat(1, 1.0)),sigma_(1.0),invsigma_(1.0) {} + Isotropic() : Diagonal(Vector::Constant(1, 1.0)),sigma_(1.0),invsigma_(1.0) {} public: diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 7ac4846f9..434d94d7f 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -155,13 +155,13 @@ TEST(NoiseModel, ConstrainedConstructors ) Vector3 mu(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? - EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); - EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas())); - EXPECT(assert_equal(gtsam::repeat(d, 0), actual->invsigmas())); // Actually zero as dummy value - EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value + EXPECT(assert_equal(Vector::Constant(d, 1000.0), actual->mu())); + EXPECT(assert_equal(Vector::Constant(d, 0), actual->sigmas())); + EXPECT(assert_equal(Vector::Constant(d, 0), actual->invsigmas())); // Actually zero as dummy value + EXPECT(assert_equal(Vector::Constant(d, 0), actual->precisions())); // Actually zero as dummy value actual = Constrained::All(d, m); - EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu())); + EXPECT(assert_equal(Vector::Constant(d, m), actual->mu())); actual = Constrained::All(d, mu); EXPECT(assert_equal(mu, actual->mu())); @@ -170,7 +170,7 @@ TEST(NoiseModel, ConstrainedConstructors ) EXPECT(assert_equal(mu, actual->mu())); actual = Constrained::MixedSigmas(m, sigmas); - EXPECT(assert_equal( gtsam::repeat(d, m), actual->mu())); + EXPECT(assert_equal(Vector::Constant(d, m), actual->mu())); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 86fb34fe0..e24c0d610 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -153,7 +153,7 @@ public: throw std::invalid_argument( "Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!"); - return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal + return Vector::Constant(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } } diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 77944da83..3664de9c5 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -69,16 +69,16 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { double alpha = 0.5; VectorValues xvalues = map_list_of // - (0, gtsam::repeat(6, 2))// - (1, gtsam::repeat(6, 4))// - (2, gtsam::repeat(6, 0))// distractor - (3, gtsam::repeat(6, 8)); + (0, Vector::Constant(6, 2))// + (1, Vector::Constant(6, 4))// + (2, Vector::Constant(6, 0))// distractor + (3, Vector::Constant(6, 8)); VectorValues yExpected = map_list_of// - (0, gtsam::repeat(6, 27))// - (1, gtsam::repeat(6, -40))// - (2, gtsam::repeat(6, 0))// distractor - (3, gtsam::repeat(6, 279)); + (0, Vector::Constant(6, 27))// + (1, Vector::Constant(6, -40))// + (2, Vector::Constant(6, 0))// distractor + (3, Vector::Constant(6, 279)); // Create full F size_t M=4, m = 3, d = 6; diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index e2f2a2a86..b1630c776 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -167,7 +167,7 @@ TEST( Similarity3, retract_first_order) { //****************************************************************************** TEST(Similarity3, localCoordinates_first_order) { - Vector d12 = repeat(7, 0.1); + Vector d12 = Vector::Constant(7, 0.1); d12(6) = 1.0; Similarity3 t1 = T1, t2 = t1.retract(d12); EXPECT(assert_equal(d12, t1.localCoordinates(t2))); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 7c1e20a1a..d4cefd329 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -73,8 +73,8 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) { EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol); EXPECT(!constraint3.isGreaterThan()); EXPECT(!constraint4.isGreaterThan()); - EXPECT(assert_equal(repeat(1, 3.0), constraint3.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1, 5.0), constraint4.evaluateError(pt2), tol)); + 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_DOUBLES_EQUAL(0.0, constraint3.error(config), tol); @@ -88,10 +88,10 @@ TEST( testBoundingConstraint, unary_basics_active1 ) { config.insert(key, pt2); EXPECT(constraint1.active(config)); EXPECT(constraint2.active(config)); - EXPECT(assert_equal(repeat(1,-3.0), constraint1.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol)); + EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol)); + EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol); EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol); } @@ -129,8 +129,8 @@ TEST( testBoundingConstraint, unary_linearization_active) { config2.insert(key, pt2); GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2); GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2); - JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), repeat(1, 3.0), hard_model1); - JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), repeat(1, 5.0), hard_model1); + JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), Vector::Constant(1, 3.0), hard_model1); + JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), Vector::Constant(1, 5.0), hard_model1); EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol)); EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol)); } diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index f72e08083..7142f72d5 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -137,7 +137,7 @@ TEST ( NonlinearEquality, error ) { actual = nle->unwhitenedError(bad_linearize); EXPECT( - assert_equal(actual, repeat(3, std::numeric_limits::infinity()))); + assert_equal(actual, Vector::Constant(3, std::numeric_limits::infinity()))); } //******************************************************************************