diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 3365525c4..c69a0ad20 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -176,11 +176,6 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) { return flag; } -/* ************************************************************************* */ -ConstSubVector sub(const Vector &v, size_t i1, size_t i2) { - return v.segment(i1,i2-i1); -} - /* ************************************************************************* */ Vector ediv_(const Vector &a, const Vector &b) { size_t n = a.size(); diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 3b0634702..d4502b6ad 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -195,15 +195,6 @@ GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector& */ GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9); -/** - * extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2 - * @param v Vector - * @param i1 first row index - * @param i2 last row index + 1 - * @return subvector v(i1:i2) - */ -GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); - /** * elementwise division, but 0/0 = 0, not inf * @param a first vector @@ -296,6 +287,7 @@ GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.siz 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 double sum(const Vector &a){return a.sum();} #endif diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 7e421573b..7e794ac04 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -126,7 +126,7 @@ TEST(Vector, sub ) a(0) = 10; a(1) = 20; a(2) = 3; a(3) = 34; a(4) = 11; a(5) = 2; - Vector result(sub(a,2,5)); + Vector result(a.segment(2,3)); Vector b(3); b(0) = 3; b(1) = 34; b(2) =11; diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 33c62cfb6..1439d4b61 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -52,7 +52,7 @@ namespace gtsam { Key key; size_t n; boost::tie(key, n) = v; - values_.insert(make_pair(key, sub(x, j, j + n))); + values_.insert(make_pair(key, x.segment(j,n))); j += n; } } diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index 2205f53bd..edd92b580 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -67,13 +67,13 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, /* ************************************************************************* */ Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { - Vector3 rho = sub(dx, 0, 3); + Vector3 rho = dx.segment(0,3); Rot3 delta_nRn = Rot3::Rodrigues(rho); Rot3 bRn = bRn_ * delta_nRn; - Vector3 x_g = x_g_ + sub(dx, 3, 6); - Vector3 x_a = x_a_ + sub(dx, 6, 9); + Vector3 x_g = x_g_ + dx.segment(3,3); + Vector3 x_a = x_a_ + dx.segment(6,3); return Mechanization_bRn2(bRn, x_g, x_a); }