From 4afe132b1aa97d29b986c7d195147bb37e592c84 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 4 Nov 2014 15:41:14 +0100 Subject: [PATCH] Fixed dimensions of Vectors --- gtsam_unstable/dynamics/FullIMUFactor.h | 28 +++++++-------- gtsam_unstable/dynamics/IMUFactor.h | 30 ++++++++-------- gtsam_unstable/dynamics/Pendulum.h | 34 +++++++++---------- gtsam_unstable/dynamics/PoseRTV.cpp | 29 ++++++++-------- gtsam_unstable/dynamics/PoseRTV.h | 6 ++-- gtsam_unstable/dynamics/VelocityConstraint3.h | 8 ++--- .../dynamics/tests/testIMUSystem.cpp | 14 ++++---- .../dynamics/tests/testPendulumFactors.cpp | 8 ++--- .../tests/testVelocityConstraint3.cpp | 23 +++++-------- .../geometry/tests/testInvDepthCamera3.cpp | 30 ++++++++-------- 10 files changed, 101 insertions(+), 109 deletions(-) diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 55dd653b0..1c35fc9b8 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -28,20 +28,20 @@ public: protected: /** measurements from the IMU */ - Vector accel_, gyro_; + Vector3 accel_, gyro_; double dt_; /// time between measurements public: /** Standard constructor */ - FullIMUFactor(const Vector& accel, const Vector& gyro, + FullIMUFactor(const Vector3& accel, const Vector3& gyro, double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) { assert(model->dim() == 9); } /** Single IMU vector - imu = [accel, gyro] */ - FullIMUFactor(const Vector& imu, + FullIMUFactor(const Vector6& imu, double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) { assert(imu.size() == 6); @@ -67,15 +67,15 @@ public: void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { std::string a = "FullIMUFactor: " + s; Base::print(a, formatter); - gtsam::print(accel_, "accel"); - gtsam::print(gyro_, "gyro"); + gtsam::print((Vector)accel_, "accel"); + gtsam::print((Vector)gyro_, "gyro"); std::cout << "dt: " << dt_ << std::endl; } // access - const Vector& gyro() const { return gyro_; } - const Vector& accel() const { return accel_; } - Vector z() const { return concatVectors(2, &accel_, &gyro_); } + const Vector3& gyro() const { return gyro_; } + const Vector3& accel() const { return accel_; } + Vector6 z() const { return (Vector6() << accel_, gyro_); } /** * Error evaluation with optional derivatives - calculates @@ -84,13 +84,13 @@ public: virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - Vector z(9); + Vector9 z; z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang - if (H1) *H1 = numericalDerivative21( + if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); - if (H2) *H2 = numericalDerivative22( + if (H2) *H2 = numericalDerivative22( boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); return z - predict_proxy(x1, x2, dt_); } @@ -106,11 +106,11 @@ public: private: /** copy of the measurement function formulated for numerical derivatives */ - static Vector3 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { - Vector hx(9); + static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { + Vector9 hx; hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang - return Vector3(hx); + return hx; } }; diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 5c821b2bf..5fb4d77aa 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -26,18 +26,18 @@ public: protected: /** measurements from the IMU */ - Vector accel_, gyro_; + Vector3 accel_, gyro_; double dt_; /// time between measurements public: /** Standard constructor */ - IMUFactor(const Vector& accel, const Vector& gyro, + IMUFactor(const Vector3& accel, const Vector3& gyro, double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {} /** Full IMU vector specification */ - IMUFactor(const Vector& imu_vector, + IMUFactor(const Vector6& imu_vector, double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) : Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {} @@ -60,15 +60,15 @@ public: void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { std::string a = "IMUFactor: " + s; Base::print(a, formatter); - gtsam::print(accel_, "accel"); - gtsam::print(gyro_, "gyro"); + gtsam::print((Vector)accel_, "accel"); + gtsam::print((Vector)gyro_, "gyro"); std::cout << "dt: " << dt_ << std::endl; } // access - const Vector& gyro() const { return gyro_; } - const Vector& accel() const { return accel_; } - Vector z() const { return concatVectors(2, &accel_, &gyro_); } + const Vector3& gyro() const { return gyro_; } + const Vector3& accel() const { return accel_; } + Vector6 z() const { return (Vector6() << accel_, gyro_);} /** * Error evaluation with optional derivatives - calculates @@ -77,10 +77,10 @@ public: virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - const Vector meas = z(); - if (H1) *H1 = numericalDerivative21( + const Vector6 meas = z(); + if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); - if (H2) *H2 = numericalDerivative22( + if (H2) *H2 = numericalDerivative22( boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); return predict_proxy(x1, x2, dt_, meas); } @@ -95,10 +95,10 @@ public: private: /** copy of the measurement function formulated for numerical derivatives */ - static Vector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, - double dt, const Vector& meas) { - Vector hx = x1.imuPrediction(x2, dt); - return Vector(meas - hx); + static Vector6 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, + double dt, const Vector6& meas) { + Vector6 hx = x1.imuPrediction(x2, dt); + return meas - hx; } }; diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 970af848c..0100e17c7 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -37,7 +37,7 @@ public: ///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0) - : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), k1, k, velKey), h_(h) {} + : Base(noiseModel::Constrained::All(1, fabs(mu)), k1, k, velKey), h_(h) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -45,15 +45,15 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); } /** q_k + h*v - q_k1 = 0, with optional derivatives */ - Vector evaluateError(double qk1, double qk, double v, + Vector evaluateError(const double& qk1, const double& qk, const double& v, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = double::Dim(); + const size_t p = 1; if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); if (H3) *H3 = eye(p)*h_; - return qk1.localCoordinates(qk.compose(double(v*h_))); + return (Vector(1) << qk+v*h_-qk1); } }; // \PendulumFactor1 @@ -85,7 +85,7 @@ public: ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0) - : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} + : Base(noiseModel::Constrained::All(1, fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -93,15 +93,15 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); } /** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ - Vector evaluateError(double vk1, double vk, double q, + Vector evaluateError(const double & vk1, const double & vk, const double & q, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = double::Dim(); + const size_t p = 1; if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); - if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value()); - return vk1.localCoordinates(double(vk - h_*g_/r_*sin(q))); + if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q); + return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1); } }; // \PendulumFactor2 @@ -135,7 +135,7 @@ public: ///Constructor PendulumFactorPk(Key pKey, Key qKey, Key qKey1, double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) - : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey, qKey, qKey1), + : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor @@ -144,11 +144,11 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); } /** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */ - Vector evaluateError(double pk, double qk, double qk1, + Vector evaluateError(const double & pk, const double & qk, const double & qk1, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = double::Dim(); + const size_t p = 1; double qmid = (1-alpha_)*qk + alpha_*qk1; double mr2_h = 1/h_*m_*r_*r_; @@ -158,7 +158,7 @@ public: if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); - return pk.localCoordinates(double(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid))); + return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk); } }; // \PendulumFactorPk @@ -191,7 +191,7 @@ public: ///Constructor PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1, double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) - : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey1, qKey, qKey1), + : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey1, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor @@ -200,11 +200,11 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); } /** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */ - Vector evaluateError(double pk1, double qk, double qk1, + Vector evaluateError(const double & pk1, const double & qk, const double & qk1, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = double::Dim(); + const size_t p = 1; double qmid = (1-alpha_)*qk + alpha_*qk1; double mr2_h = 1/h_*m_*r_*r_; @@ -214,7 +214,7 @@ public: if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); - return pk1.localCoordinates(double(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid))); + return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1); } }; // \PendulumFactorPk1 diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 2246baee1..ed8d54696 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -57,18 +57,17 @@ void PoseRTV::print(const string& s) const { } /* ************************************************************************* */ -PoseRTV PoseRTV::Expmap(const Vector& v) { - assert(v.size() == 9); - Pose3 newPose = Pose3::Expmap(sub(v, 0, 6)); - Velocity3 newVel = Velocity3::Expmap(sub(v, 6, 9)); +PoseRTV PoseRTV::Expmap(const Vector9& v) { + Pose3 newPose = Pose3::Expmap(v.head<6>()); + Velocity3 newVel = Velocity3::Expmap(v.tail<3>()); return PoseRTV(newPose, newVel); } /* ************************************************************************* */ -Vector PoseRTV::Logmap(const PoseRTV& p) { - Vector Lx = Pose3::Logmap(p.Rt_); - Vector Lv = Velocity3::Logmap(p.v_); - return concatVectors(2, &Lx, &Lv); +Vector9 PoseRTV::Logmap(const PoseRTV& p) { + Vector6 Lx = Pose3::Logmap(p.Rt_); + Vector3 Lv = Velocity3::Logmap(p.v_); + return (Vector9() << Lx, Lv); } /* ************************************************************************* */ @@ -84,9 +83,9 @@ PoseRTV PoseRTV::retract(const Vector& v) const { Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { const Pose3& x0 = pose(), &x1 = p1.pose(); // First order approximation - Vector poseLogmap = x0.localCoordinates(x1); - Vector lv = rotation().unrotate(p1.velocity() - v_).vector(); - return concatVectors(2, &poseLogmap, &lv); + Vector6 poseLogmap = x0.localCoordinates(x1); + Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector(); + return (Vector(9) << poseLogmap, lv); } /* ************************************************************************* */ @@ -190,16 +189,16 @@ PoseRTV PoseRTV::generalDynamics( } /* ************************************************************************* */ -Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { +Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { // split out states const Rot3 &r1 = R(), &r2 = x2.R(); const Velocity3 &v1 = v(), &v2 = x2.v(); - Vector imu(6); + Vector6 imu; // acceleration Vector accel = v1.localCoordinates(v2) / dt; - imu.head(3) = r2.transpose() * (accel - g); + imu.head<3>() = r2.transpose() * (accel - g); // rotation rates // just using euler angles based on matlab code @@ -211,7 +210,7 @@ Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { // normalize yaw in difference (as per Mitch's code) dR(2) = Rot2::fromAngle(dR(2)).theta(); dR /= dt; - imu.tail(3) = Enb * dR; + imu.tail<3>() = Enb * dR; // imu.tail(3) = r1.transpose() * dR; return imu; diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index ae4ddade4..43d018752 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -86,8 +86,8 @@ public: * expmap/logmap are poor approximations that assume independence of components * Currently implemented using the poor retract/unretract approximations */ - static PoseRTV Expmap(const Vector& v); - static Vector Logmap(const PoseRTV& p); + static PoseRTV Expmap(const Vector9& v); + static Vector9 Logmap(const PoseRTV& p); static PoseRTV identity() { return PoseRTV(); } @@ -129,7 +129,7 @@ public: /// Dynamics predictor for both ground and flying robots, given states at 1 and 2 /// Always move from time 1 to time 2 /// @return imu measurement, as [accel, gyro] - Vector imuPrediction(const PoseRTV& x2, double dt) const; + Vector6 imuPrediction(const PoseRTV& x2, double dt) const; /// predict measurement and where Point3 for x2 should be, as a way /// of enforcing a velocity constraint diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 74cddff10..1a432ba1e 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -27,7 +27,7 @@ public: ///TODO: comment VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0) - : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {} + : Base(noiseModel::Constrained::All(1, fabs(mu)), key1, key2, velKey), dt_(dt) {} virtual ~VelocityConstraint3() {} /// @return a deep copy of this factor @@ -36,15 +36,15 @@ public: gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); } /** x1 + v*dt - x2 = 0, with optional derivatives */ - Vector evaluateError(double x1, double x2, double v, + Vector evaluateError(const double& x1, const double& x2, const double& v, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = double::Dim(); + const size_t p = 1; if (H1) *H1 = eye(p); if (H2) *H2 = -eye(p); if (H3) *H3 = eye(p)*dt_; - return x2.localCoordinates(x1.compose(double(v*dt_))); + return (Vector(1) << x1+v*dt_-x2); } private: diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index be7078d9a..550b0e33c 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -62,10 +62,9 @@ TEST( testIMUSystem, optimize_chain ) { // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); - Vector imu12(6), imu23(6), imu34(6); - imu12 = pose1.imuPrediction(pose2, dt); - imu23 = pose2.imuPrediction(pose3, dt); - imu34 = pose3.imuPrediction(pose4, dt); + Vector6 imu12 = pose1.imuPrediction(pose2, dt); + Vector6 imu23 = pose2.imuPrediction(pose3, dt); + Vector6 imu34 = pose3.imuPrediction(pose4, dt); // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; @@ -109,10 +108,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); - Vector imu12(6), imu23(6), imu34(6); - imu12 = pose1.imuPrediction(pose2, dt); - imu23 = pose2.imuPrediction(pose3, dt); - imu34 = pose3.imuPrediction(pose4, dt); + Vector6 imu12 = pose1.imuPrediction(pose2, dt); + Vector6 imu23 = pose2.imuPrediction(pose3, dt); + Vector6 imu34 = pose3.imuPrediction(pose4, dt); // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp index ce176787c..5a4593270 100644 --- a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp +++ b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp @@ -18,8 +18,8 @@ namespace { const double g = 9.81, l = 1.0; const double deg2rad = M_PI/180.0; - LieScalar origin, q1(deg2rad*30.0), q2(deg2rad*31.0); - LieScalar v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1))); + double q1(deg2rad*30.0), q2(deg2rad*31.0); + double v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1))); } @@ -46,7 +46,7 @@ TEST( testPendulumFactorPk, evaluateError) { // hard constraints don't need a noise model PendulumFactorPk constraint(P(1), Q(1), Q(2), h); - LieScalar pk( 1/h * (q2-q1) + h*g*sin(q1) ); + double pk( 1/h * (q2-q1) + h*g*sin(q1) ); // verify error function EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol)); @@ -57,7 +57,7 @@ TEST( testPendulumFactorPk1, evaluateError) { // hard constraints don't need a noise model PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h); - LieScalar pk1( 1/h * (q2-q1) ); + double pk1( 1/h * (q2-q1) ); // verify error function EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol)); diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp index db4b7c586..ac27ae563 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp @@ -8,23 +8,16 @@ #include #include -/* ************************************************************************* */ using namespace gtsam; - -namespace { - - const double tol=1e-5; - const double dt = 1.0; - - LieScalar origin, - x1(1.0), x2(2.0), v(1.0); - -} +using namespace gtsam::symbol_shorthand; /* ************************************************************************* */ +// evaluateError TEST( testVelocityConstraint3, evaluateError) { - using namespace gtsam::symbol_shorthand; + const double tol = 1e-5; + const double dt = 1.0; + double x1(1.0), x2(2.0), v(1.0); // hard constraints don't need a noise model VelocityConstraint3 constraint(X(1), X(2), V(1), dt); @@ -33,7 +26,9 @@ TEST( testVelocityConstraint3, evaluateError) { EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol)); } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 3385998b0..0fc664715 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -30,9 +30,9 @@ TEST( InvDepthFactor, Project1) { InvDepthCamera3 inv_camera(level_pose, K); Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); - LieScalar inv_depth(1./4); + double inv_depth(1./4); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); - EXPECT(assert_equal(expected_uv, actual_uv)); + EXPECT_DOUBLES_EQUAL(expected_uv, actual_uv,1e-8); EXPECT(assert_equal(Point2(640,480), actual_uv)); } @@ -46,7 +46,7 @@ TEST( InvDepthFactor, Project2) { InvDepthCamera3 inv_camera(level_pose, K); Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); - LieScalar inv_depth(1/sqrt(3.0)); + double inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } @@ -61,7 +61,7 @@ TEST( InvDepthFactor, Project3) { InvDepthCamera3 inv_camera(level_pose, K); Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); - LieScalar inv_depth( 1./sqrt(1.0+1+4)); + double inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } @@ -76,20 +76,20 @@ TEST( InvDepthFactor, Project4) { InvDepthCamera3 inv_camera(level_pose, K); Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); - LieScalar inv_depth(1./sqrt(1.+16.+4.)); + double inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -Point2 project_(const Pose3& pose, const Vector5& landmark, const LieScalar& inv_depth) { +Point2 project_(const Pose3& pose, const Vector5& landmark, const double& inv_depth) { return InvDepthCamera3(pose,K).project(landmark, inv_depth); } TEST( InvDepthFactor, Dproject_pose) { Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); - LieScalar inv_depth(1./4); + double inv_depth(1./4); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; @@ -101,7 +101,7 @@ TEST( InvDepthFactor, Dproject_pose) TEST( InvDepthFactor, Dproject_landmark) { Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); - LieScalar inv_depth(1./4); + double inv_depth(1./4); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; @@ -113,7 +113,7 @@ TEST( InvDepthFactor, Dproject_landmark) TEST( InvDepthFactor, Dproject_inv_depth) { Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); - LieScalar inv_depth(1./4); + double inv_depth(1./4); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; @@ -125,15 +125,15 @@ TEST( InvDepthFactor, Dproject_inv_depth) TEST(InvDepthFactor, backproject) { Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); - LieScalar inv_depth(1./4); + double inv_depth(1./4); InvDepthCamera3 inv_camera(level_pose,K); Point2 z = inv_camera.project(expected, inv_depth); Vector5 actual_vec; - LieScalar actual_inv; + double actual_inv; boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4); EXPECT(assert_equal(expected,actual_vec,1e-7)); - EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); + EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7); } /* ************************************************************************* */ @@ -141,15 +141,15 @@ TEST(InvDepthFactor, backproject2) { // backwards facing camera Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); - LieScalar inv_depth(1./10); + double inv_depth(1./10); InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); Vector5 actual_vec; - LieScalar actual_inv; + double actual_inv; boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10); EXPECT(assert_equal(expected,actual_vec,1e-7)); - EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); + EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7); } /* ************************************************************************* */