From 2fe0c26f4e998a1277650e81a3a89cf2c8d4a6a9 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 15 Apr 2016 20:01:22 -0400 Subject: [PATCH] Deprecated delta() and basis(). All unit tests pass. --- gtsam/base/Matrix.cpp | 2 +- gtsam/base/Vector.cpp | 2 +- gtsam/base/Vector.h | 4 +--- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam_unstable/dynamics/DynamicsPriors.h | 2 +- gtsam_unstable/dynamics/PoseRTV.cpp | 4 ++-- gtsam_unstable/dynamics/tests/testIMUSystem.cpp | 6 +++--- .../dynamics/tests/testVelocityConstraint.cpp | 12 ++++++------ gtsam_unstable/geometry/SimPolygon2D.cpp | 2 +- 9 files changed, 17 insertions(+), 19 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index ff509b41e..a9d521a7f 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -290,7 +290,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { if (precision < 1e-8) continue; // create solution and copy into r - Vector r(basis(n, j)); + Vector r(Vector::Unit(n,j)); for (size_t j2=j+1; j2& vs); */ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 inline Vector abs(const Vector& v){return v.cwiseAbs();} inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} @@ -252,10 +251,9 @@ inline double sum(const Vector &a){return a.sum();} 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 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); } - +#endif } // namespace gtsam #include diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 165870f25..97ccbcb34 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -712,7 +712,7 @@ TEST(Pose3, Bearing2) { TEST( Pose3, unicycle ) { // velocity in X should be X in inertial frame, rather than global frame - Vector x_step = delta(6,3,1.0); + Vector x_step = Vector::Unit(6,3)*1.0; EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index b4dfa7dc8..1e768ef2a 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -75,7 +75,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { */ DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model) : Base(key, model) { - this->prior_ = delta(4, 0, height); // [z, vz, roll, pitch] + this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch] this->mask_.resize(4); this->mask_[0] = 5; // z = height this->mask_[1] = 8; // vz diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index b6fc61411..c1afe3882 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -11,7 +11,7 @@ namespace gtsam { using namespace std; -static const Vector kGravity = delta(3, 2, 9.81); +static const Vector kGravity = Vector::Unit(3,2)*9.81; /* ************************************************************************* */ double bound(double a, double min, double max) { @@ -105,7 +105,7 @@ PoseRTV PoseRTV::flyingDynamics( Vector Acc_n = yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1 - + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch + + Vector::Unit(3,2)*(loss_lift - lift_control); // falling due to lift lost from pitch // Update Vehicle Position and Velocity Velocity3 v2 = v1 + Velocity3(Acc_n * dt); diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 5fe29a57f..494f2731f 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -26,7 +26,7 @@ using namespace gtsam; const double tol=1e-5; static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4; -static const Vector g = delta(3, 2, -9.81); +static const Vector g = Vector::Unit(3,2)*(-9.81); /* ************************************************************************* */ TEST(testIMUSystem, instantiations) { @@ -149,8 +149,8 @@ TEST( testIMUSystem, linear_trajectory) { const double dt = 1.0; PoseRTV start; - Vector accel = delta(3, 0, 0.5); // forward force - Vector gyro = delta(3, 0, 0.1); // constant rotation + Vector accel = Vector::Unit(3,0)*0.5; // forward force + Vector gyro = Vector::Unit(3,0)*0.1; // constant rotation SharedDiagonal model = noiseModel::Unit::Create(9); Values true_traj, init_traj; diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 429edb760..6d8206177 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -27,8 +27,8 @@ TEST( testVelocityConstraint, trapezoidal ) { // verify error function 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)); + EXPECT(assert_equal(Vector::Unit(3,0)*(-1.0), constraint.evaluateError(pose1, pose1), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ @@ -37,10 +37,10 @@ TEST( testEulerVelocityConstraint, euler_start ) { VelocityConstraint constraint(x1, x2, dynamics::EULER_START, dt); // verify error function - EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1), 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)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ @@ -49,10 +49,10 @@ TEST( testEulerVelocityConstraint, euler_end ) { VelocityConstraint constraint(x1, x2, dynamics::EULER_END, dt); // verify error function - EXPECT(assert_equal(delta(3, 0,-0.5), constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*(-0.5), constraint.evaluateError(origin, pose1), 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)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index f60ea107c..90e3eeea6 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -149,7 +149,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( // extend from B to find C double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); - Pose2 xC = xB.retract(delta(3, 0, dBC)); + Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC); // use triangle equality to verify non-degenerate triangle double dAC = xA.t().distance(xC.t());