From 6587f9781c725f25d5789fe51b22a77bdb8a970a Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Tue, 22 Oct 2013 04:22:50 +0000 Subject: [PATCH] Fix Vector_() to Vec() in gtsam_unstable/dynamics --- gtsam_unstable/dynamics/PoseRTV.cpp | 6 +++--- gtsam_unstable/dynamics/SimpleHelicopter.h | 4 ++-- gtsam_unstable/dynamics/tests/testIMUSystem.cpp | 12 ++++++------ gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 10 +++++----- .../dynamics/tests/testSimpleHelicopter.cpp | 14 +++++++------- .../dynamics/tests/testVelocityConstraint.cpp | 4 ++-- 6 files changed, 25 insertions(+), 25 deletions(-) diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 99964c9db..18666d117 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -126,7 +126,7 @@ PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate, const Velocity3& v1 = v(); // Update vehicle heading - Rot3 r2 = r1.retract(Vector_(3, 0.0, 0.0, heading_rate * dt)); + Rot3 r2 = r1.retract((Vec(3) << 0.0, 0.0, heading_rate * dt)); const double yaw2 = r2.ypr()(0); // Update vehicle position @@ -150,7 +150,7 @@ PoseRTV PoseRTV::flyingDynamics( const Velocity3& v1 = v(); // Update vehicle heading (and normalise yaw) - Vector rot_rates = Vector_(3, 0.0, pitch_rate, heading_rate); + Vector rot_rates = (Vec(3) << 0.0, pitch_rate, heading_rate); Rot3 r2 = r1.retract(rot_rates*dt); // Work out dynamics on platform @@ -165,7 +165,7 @@ PoseRTV PoseRTV::flyingDynamics( Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame - - drag * Vector_(3, v1.x(), v1.y(), 0.0) // drag term dependent on v1 + - drag * (Vec(3) << v1.x(), v1.y(), 0.0) // drag term dependent on v1 + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch // Update Vehicle Position and Velocity diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 94bbc6092..59ef2b9de 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -126,7 +126,7 @@ public: Matrix D_gravityBody_gk; Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk); - Vector f_ext = Vector_(6, 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); + Vector f_ext = (Vec(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; @@ -154,7 +154,7 @@ public: Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1; Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_)); - Vector f_ext = Vector_(6, 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); + Vector f_ext = (Vec(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index ddee01b9b..9807d92c9 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Vector_(3, 2.0, 2.0, 0.0)), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Vector_(3, 0.0, 0.0, 0.0)), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Vector_(3, 2.0, 2.0, 0.0)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)(Vec(3) << 2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)(Vec(3) << 0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)(Vec(3) << 2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -103,9 +103,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index b362f2ca2..c6e9c8439 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -47,7 +47,7 @@ TEST( testPoseRTV, constructors ) { EXPECT(assert_equal(Velocity3(), state4.v(), tol)); EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol)); - Vector vec_init = Vector_(9, 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6); + Vector vec_init = (Vec(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6); PoseRTV state5(vec_init); EXPECT(assert_equal(pt, state5.t(), tol)); EXPECT(assert_equal(rot, state5.R(), tol)); @@ -83,7 +83,7 @@ TEST( testPoseRTV, Lie ) { EXPECT(assert_equal(state1, state1.retract(zero(9)), tol)); EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); - Vector delta = Vector_(9, 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3); + Vector delta = (Vec(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3); Rot3 rot2 = rot.retract(repeat(3, 0.1)); Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); @@ -99,7 +99,7 @@ TEST( testPoseRTV, dynamics_identities ) { PoseRTV x0, x1, x2, x3, x4; const double dt = 0.1; - Vector accel = Vector_(3, 0.2, 0.0, 0.0), gyro = Vector_(3, 0.0, 0.0, 0.2); + Vector accel = (Vec(3) << 0.2, 0.0, 0.0), gyro = (Vec(3) << 0.0, 0.0, 0.2); Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6); x1 = x0.generalDynamics(accel, gyro, dt); @@ -181,14 +181,14 @@ TEST( testPoseRTV, transformed_from_2 ) { TEST(testPoseRTV, RRTMbn) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol)); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMbn(Vector_(3, 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMbn((Vec(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol)); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMnb(Vector_(3, 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMnb((Vec(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 09f67f527..b7717b1aa 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -18,19 +18,19 @@ const double deg2rad = M_PI/180.0; //Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); -//LieVector v1(Vector_(6, 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); -LieVector V1_w(Vector_(6, 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); +//LieVector v1((Vec(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); +LieVector V1_w((Vec(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); LieVector V1_g1 = g1.inverse().Adjoint(V1_w); Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); //LieVector v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; -Vector gamma2 = Vector_(2, 0.0, 0.0); // no shape -Vector u2 = Vector_(2, 0.0, 0.0); // no control at time 2 +Vector gamma2 = (Vec(2) << 0.0, 0.0); // no shape +Vector u2 = (Vec(2) << 0.0, 0.0); // no control at time 2 double distT = 1.0; // distance from the body-centered x axis to the big top motor double distR = 5.0; // distance from the body-centered z axis to the small motor -Matrix Mass = diag(Vector_(3, mass, mass, mass)); -Matrix Inertia = diag(Vector_(6, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass)); +Matrix Mass = diag((Vec(3) << mass, mass, mass)); +Matrix Inertia = diag((Vec(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass)); Vector computeFu(const Vector& gamma, const Vector& control) { double gamma_r = gamma(0), gamma_p = gamma(1); @@ -100,7 +100,7 @@ TEST( Reconstruction, evaluateError) { /* ************************************************************************* */ // Implement Newton-Euler equation for rigid body dynamics Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { - Matrix W = Pose3::adjointMap(Vector_(6, Vb(0), Vb(1), Vb(2), 0., 0., 0.)); + Matrix W = Pose3::adjointMap((Vec(6) << Vb(0), Vb(1), Vb(2), 0., 0., 0.)); Vector dV = Inertia.inverse()*(Fb - W*Inertia*Vb); return dV; } diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index c6c6d3219..90b68dc0e 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2; const double dt = 1.0; PoseRTV origin, - pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0)), + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vec(3) << 1.0, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)), - pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0)); + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vec(3) << 1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) {