From 708d114b3c1812c1b0e399d0851027c00c7f3c3c Mon Sep 17 00:00:00 2001 From: krunalchande Date: Wed, 19 Nov 2014 11:59:08 -0500 Subject: [PATCH] Moved project specific factors into a different project. --- gtsam.h | 23 ---- gtsam/slam/CageFactor.h | 98 -------------- gtsam/slam/DistanceFactor.h | 88 ------------- gtsam/slam/DroneDynamicsFactor.h | 114 ---------------- gtsam/slam/DroneDynamicsVelXYFactor.h | 124 ------------------ gtsam/slam/tests/testCageFactor.cpp | 78 ----------- gtsam/slam/tests/testDistanceFactor.cpp | 82 ------------ gtsam/slam/tests/testDroneDynamicsFactor.cpp | 102 -------------- .../tests/testDroneDynamicsVelXYFactor.cpp | 108 --------------- 9 files changed, 817 deletions(-) delete mode 100644 gtsam/slam/CageFactor.h delete mode 100644 gtsam/slam/DistanceFactor.h delete mode 100644 gtsam/slam/DroneDynamicsFactor.h delete mode 100644 gtsam/slam/DroneDynamicsVelXYFactor.h delete mode 100644 gtsam/slam/tests/testCageFactor.cpp delete mode 100644 gtsam/slam/tests/testDistanceFactor.cpp delete mode 100644 gtsam/slam/tests/testDroneDynamicsFactor.cpp delete mode 100644 gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp diff --git a/gtsam.h b/gtsam.h index 75310e512..4438bb363 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2101,15 +2101,6 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { }; -#include -template -virtual class DistanceFactor : gtsam::NoiseModelFactor { - DistanceFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - #include template @@ -2139,20 +2130,6 @@ typedef gtsam::RangeFactor RangeFactorSimple typedef gtsam::RangeFactor RangeFactorCalibratedCamera; typedef gtsam::RangeFactor RangeFactorSimpleCamera; -#include -virtual class DroneDynamicsFactor : gtsam::NoiseModelFactor { - DroneDynamicsFactor(size_t key1, size_t key2, const gtsam::LieVector& measured, const gtsam::noiseModel::Base* noiseModel); -}; - -#include -virtual class DroneDynamicsVelXYFactor : gtsam::NoiseModelFactor { - DroneDynamicsVelXYFactor(size_t key1, size_t key2, size_t key3, Vector motors, Vector acc, const gtsam::noiseModel::Base* noiseModel); -}; - -#include -virtual class CageFactor : gtsam::NoiseModelFactor { - CageFactor(size_t key1, const gtsam::Pose3& pose, double cageBoundary, const gtsam::noiseModel::Base* noiseModel); -}; #include template diff --git a/gtsam/slam/CageFactor.h b/gtsam/slam/CageFactor.h deleted file mode 100644 index 54a77b541..000000000 --- a/gtsam/slam/CageFactor.h +++ /dev/null @@ -1,98 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file CageFactor.h - * @author Krunal Chande - * @date November 10, 2014 - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -/** - * Factor to constrain position based on size of the accessible area - */ - -class CageFactor: public NoiseModelFactor1 { -private: - Pose3 pose_; - double cageBoundary_; - typedef CageFactor This; - typedef NoiseModelFactor1 Base; - -public: - CageFactor() {} /* Default Constructor*/ - CageFactor(Key poseKey, const Pose3& pose, double cageBoundary, const SharedNoiseModel& model): - Base(model, poseKey), pose_(pose), cageBoundary_(cageBoundary){} - virtual ~CageFactor(){} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast(gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - /** h(x) - z */ - Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { - - double distance = pose.translation().dist(Point3(0,0,0)); - if(distance > cageBoundary_){ - double distance = pose.range(Point3(0,0,0), H); - return (gtsam::Vector(1) << distance - cageBoundary_); - } else { - if(H) *H = gtsam::zeros(1, Pose3::Dim()); - return (gtsam::Vector(1) << 0.0); - } -// Point3 p2; -// double x = pose.x(), y = pose.y(), z = pose.z(); -// if (x < 0) x = -x; -// if (y < 0) y = -y; -// if (z < 0) z = -z; -// double errorX = 100/(x-cageBoundary_), errorY = 100/(y-cageBoundary_), errorZ = 100/(z-cageBoundary_); -// if (H) *H = pose.translation().distance(p2, H); -// return (Vector(3)< (&expected); - return e != NULL - && Base::equals(*e, tol) - ; - } - - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "Cage Factor, Cage Boundary = " << cageBoundary_ << " Pose: " << pose_ << std::endl; - Base::print("", keyFormatter); - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(cageBoundary_); - ar & BOOST_SERIALIZATION_NVP(pose_); - } - -}; // end CageFactor -} // end namespace - - diff --git a/gtsam/slam/DistanceFactor.h b/gtsam/slam/DistanceFactor.h deleted file mode 100644 index acecd41a2..000000000 --- a/gtsam/slam/DistanceFactor.h +++ /dev/null @@ -1,88 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file DistanceFactor.h - * @author Duy-Nguyen Ta - * @date Sep 26, 2014 - * - */ - -#pragma once - -#include - -namespace gtsam { - -/** - * Factor to constrain known measured distance between two points - */ -template -class DistanceFactor: public NoiseModelFactor2 { - - double measured_; /// measured distance - - typedef NoiseModelFactor2 Base; - typedef DistanceFactor This; - -public: - /// Default constructor - DistanceFactor() { - } - - /// Constructor with keys and known measured distance - DistanceFactor(Key p1, Key p2, double measured, const SharedNoiseModel& model) : - Base(model, p1, p2), measured_(measured) { - } - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /// h(x)-z - Vector evaluateError(const POINT& p1, const POINT& p2, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - double distance = p1.distance(p2, H1, H2); - return (Vector(1) << distance - measured_); - } - - /** return the measured */ - double measured() const { - return measured_; - } - - /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol; - } - - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "DistanceFactor, distance = " << measured_ << std::endl; - Base::print("", keyFormatter); - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } -}; - -} /* namespace gtsam */ diff --git a/gtsam/slam/DroneDynamicsFactor.h b/gtsam/slam/DroneDynamicsFactor.h deleted file mode 100644 index e471e0172..000000000 --- a/gtsam/slam/DroneDynamicsFactor.h +++ /dev/null @@ -1,114 +0,0 @@ - - -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file DroneDynamicsFactor.h - * @author Duy-Nguyen Ta - * @date Sep 29, 2014 - */ -// Implementation is incorrect use DroneDynamicsVelXYFactor instead. -#pragma once - -#include -#include -#include -#include -#include - -namespace gtsam { - - /** - * Binary factor for a range measurement - * @addtogroup SLAM - */ - class DroneDynamicsFactor: public NoiseModelFactor2 { - private: - - LieVector measured_; /** body velocity measured from raw acc and motor inputs*/ - - typedef DroneDynamicsFactor This; - typedef NoiseModelFactor2 Base; - - public: - - DroneDynamicsFactor() {} /* Default constructor */ - - DroneDynamicsFactor(Key poseKey, Key velKey, const LieVector& measured, - const SharedNoiseModel& model) : - Base(model, poseKey, velKey), measured_(measured) { - } - - virtual ~DroneDynamicsFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** h(x)-z */ - Vector evaluateError(const Pose3& pose, const LieVector& vel, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - - // error = v - wRb*measured - Rot3 wRb = pose.rotation(); - Vector3 error; - - if (H1 || H2) { - *H2 = eye(3); - *H1 = zeros(3,6); - Matrix H1Rot; - error = wRb.unrotate(Point3(vel.vector()), H1Rot, H2).vector() - measured_.vector(); - (*H1).block(0,0,3,3) = H1Rot; - } - else { - error = wRb.unrotate(Point3(vel.vector())).vector() - measured_.vector(); - } - - return error; - } - - /** return the measured */ - LieVector measured() const { - return measured_; - } - - /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL - && Base::equals(*e, tol) - ; - } - - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "DroneDynamicsFactor, measured = " << measured_.vector().transpose() << std::endl; - Base::print("", keyFormatter); - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } - }; // DroneDynamicsFactor - -} // namespace gtsam - - - diff --git a/gtsam/slam/DroneDynamicsVelXYFactor.h b/gtsam/slam/DroneDynamicsVelXYFactor.h deleted file mode 100644 index fc60965b2..000000000 --- a/gtsam/slam/DroneDynamicsVelXYFactor.h +++ /dev/null @@ -1,124 +0,0 @@ - - -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file DroneDynamicsFactor.h - * @author Duy-Nguyen Ta - * @date Oct 1, 2014 - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace gtsam { - - /** - * Binary factor for a range measurement - * @addtogroup SLAM - */ - class DroneDynamicsVelXYFactor: public NoiseModelFactor3 { - private: - - Vector motors_; /** motor inputs */ - Vector acc_; /** raw acc */ - Matrix M_; - - typedef DroneDynamicsVelXYFactor This; - typedef NoiseModelFactor3 Base; - - public: - - DroneDynamicsVelXYFactor() {} /* Default constructor */ - - DroneDynamicsVelXYFactor(Key poseKey, Key velKey, Key cKey, const Vector& motors, const Vector& acc, - const SharedNoiseModel& model) : - Base(model, poseKey, velKey, cKey), motors_(motors), acc_(acc), M_(computeM(motors, acc)) { - } - - virtual ~DroneDynamicsVelXYFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - // M = [sum(sqrt(m))ax 1 0 0; 0 0 sum(sqrt(m))ay 1; 0 0 0 0] - Matrix computeM(const Vector& motors, const Vector& acc) const { - Matrix M = zeros(3,4); - double sumMotors = (motors(0)) + (motors(1)) + (motors(2)) + (motors(3)); - M(0,0) = acc(0)/sumMotors; M(0, 1) = 1.0/sumMotors; - M(1,2) = 1.0/sumMotors; M(1, 3) = acc(1)/sumMotors; - return M; - } - - /** h(x)-z */ - Vector evaluateError(const Pose3& pose, const LieVector& vel, const LieVector& c, - boost::optional H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { - - // error = R'*v - M*c, where - Rot3 wRb = pose.rotation(); - Vector error; - - if (H1 || H2 || H3) { - *H1 = zeros(3, 6); - *H2 = eye(3); - Matrix H1Rot; - error = wRb.unrotate(Point3(vel.vector()), H1Rot, H2).vector() - M_*c.vector(); - (*H1).block(0,0,3,3) = H1Rot; - - *H3 = -M_; - } - else { - error = wRb.unrotate(Point3(vel.vector())).vector() - M_*c.vector(); - } - - return error; - } - - /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL - && Base::equals(*e, tol) - ; - } - - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "DroneDynamicsVelXYFactor, motors = " << motors_.transpose() << " acc: " << acc_.transpose() << std::endl; - Base::print("", keyFormatter); - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(motors_); - ar & BOOST_SERIALIZATION_NVP(acc_); - } - }; // DroneDynamicsVelXYFactor - -} // namespace gtsam - - - diff --git a/gtsam/slam/tests/testCageFactor.cpp b/gtsam/slam/tests/testCageFactor.cpp deleted file mode 100644 index 3379aa701..000000000 --- a/gtsam/slam/tests/testCageFactor.cpp +++ /dev/null @@ -1,78 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testCageFactor.cpp - * @brief Unit tests CageFactor class - * @author Krunal Chande - */ - - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -// Create a noise model -static SharedNoiseModel model(noiseModel::Unit::Create(6)); - -LieVector factorError(const Pose3& pose, const CageFactor& factor) { - return factor.evaluateError(pose); -} - - -/* ************************************************************************* */ -TEST(CageFactor, Inside) { - Key poseKey(1); - Pose3 pose(Rot3::ypr(0,0,0),Point3(0,0,0)); - double cageBoundary = 10; // in m - CageFactor factor(poseKey, pose, cageBoundary, model); - - // Set the linearization point - Pose3 poseLin; - Matrix H; - Vector actualError(factor.evaluateError(poseLin, H)); - Vector expectedError = zero(1); - CHECK(assert_equal(expectedError, actualError, 1e-9)); - - // use numerical derivatives to calculate the jacobians - Matrix HExpected; - HExpected = numericalDerivative11(boost::bind(&factorError, _1, factor), pose); - CHECK(assert_equal(HExpected, H, 1e-9)); -} - -/* ************************************************************************* */ -TEST(CageFactor, Outside) { - Key poseKey(1); - Point3 translation = Point3(15,0,0); - Pose3 pose(Rot3::ypr(0,0,0),translation); - double cageBoundary = 10; // in m - CageFactor factor(poseKey, pose, cageBoundary, model); - - // Set the linearization point - Pose3 poseLin; - Matrix H; - Vector actualError(factor.evaluateError(pose, H)); - Vector expectedError(Vector(1)<<5); - CHECK(assert_equal(expectedError, actualError, 1e-9)); - - // use numerical derivatives to calculate the jacobians - Matrix HExpected; - HExpected = numericalDerivative11(boost::bind(&factorError, _1, factor), pose); - CHECK(assert_equal(HExpected, H, 1e-9)); -} -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testDistanceFactor.cpp b/gtsam/slam/tests/testDistanceFactor.cpp deleted file mode 100644 index b4c35a98f..000000000 --- a/gtsam/slam/tests/testDistanceFactor.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testDistanceFactor.cpp - * @brief Unit tests for DistanceFactor Class - * @author Duy-Nguyen Ta - * @date Oct 2014 - */ - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -typedef DistanceFactor DistanceFactor2D; -typedef DistanceFactor DistanceFactor3D; - -SharedDiagonal noise = noiseModel::Unit::Create(1); -Point3 P(0., 1., 2.5), Q(10., -81., 7.); -Point2 p(1., 2.5), q(-81., 7.); - -/* ************************************************************************* */ -TEST(DistanceFactor, Point3) { - DistanceFactor3D distanceFactor(0, 1, P.distance(Q), noise); - Matrix H1, H2; - Vector error = distanceFactor.evaluateError(P, Q, H1, H2); - - Vector expectedError = zero(1); - EXPECT(assert_equal(expectedError, error, 1e-10)); - - boost::function testEvaluateError( - boost::bind(&DistanceFactor3D::evaluateError, distanceFactor, _1, _2, - boost::none, boost::none)); - Matrix numericalH1 = numericalDerivative21(testEvaluateError, P, Q, 1e-5); - Matrix numericalH2 = numericalDerivative22(testEvaluateError, P, Q, 1e-5); - - EXPECT(assert_equal(numericalH1, H1, 1e-8)); - EXPECT(assert_equal(numericalH2, H2, 1e-8)); - -} - -/* ************************************************************************* */ -TEST(DistanceFactor, Point2) { - DistanceFactor2D distanceFactor(0, 1, p.distance(q), noise); - Matrix H1, H2; - Vector error = distanceFactor.evaluateError(p, q, H1, H2); - - Vector expectedError = zero(1); - EXPECT(assert_equal(expectedError, error, 1e-10)); - - boost::function testEvaluateError( - boost::bind(&DistanceFactor2D::evaluateError, distanceFactor, _1, _2, - boost::none, boost::none)); - Matrix numericalH1 = numericalDerivative21(testEvaluateError, p, q, 1e-5); - Matrix numericalH2 = numericalDerivative22(testEvaluateError, p, q, 1e-5); - - EXPECT(assert_equal(numericalH1, H1, 1e-8)); - EXPECT(assert_equal(numericalH2, H2, 1e-8)); - -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testDroneDynamicsFactor.cpp b/gtsam/slam/tests/testDroneDynamicsFactor.cpp deleted file mode 100644 index bf11ed805..000000000 --- a/gtsam/slam/tests/testDroneDynamicsFactor.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testRangeFactor.cpp - * @brief Unit tests for DroneDynamicsFactor Class - * @author Duy-Nguyen Ta - * @date Oct 2014 - */ - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -// Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(3)); - -/* ************************************************************************* */ -LieVector factorError(const Pose3& pose, const LieVector& vel, const DroneDynamicsFactor& factor) { - return factor.evaluateError(pose, vel); -} - -/* ************************************************************************* */ -TEST( DroneDynamicsFactor, Error) { - // Create a factor - Key poseKey(1); - Key velKey(2); - LieVector measurement((Vector(3)<<10.0, 1.5, 0.0)); - DroneDynamicsFactor factor(poseKey, velKey, measurement, model); - - // Set the linearization point - Pose3 pose(Rot3::ypr(1.0, 2.0, 0.57), Point3()); - LieVector vel((Vector(3) << - -2.913425624770731, - -2.200086236883632, - -9.429823523226959)); - - // Use the factor to calculate the error - Matrix H1, H2; - Vector actualError(factor.evaluateError(pose, vel, H1, H2)); - - Vector expectedError = zero(3); - - // Verify we get the expected error - CHECK(assert_equal(expectedError, actualError, 1e-9)); - - - // Use numerical derivatives to calculate the Jacobians - Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError, _1, vel, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError, pose, _1, factor), vel); - - // Verify the Jacobians are correct - CHECK(assert_equal(H1Expected, H1, 1e-9)); - CHECK(assert_equal(H2Expected, H2, 1e-9)); -} - -/* ************************************************************************* -TEST( DroneDynamicsFactor, Jacobian2D ) { - // Create a factor - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); - RangeFactor2D factor(poseKey, pointKey, measurement, model); - - // Set the linearization point - Pose2 pose(1.0, 2.0, 0.57); - Point2 point(-4.0, 11.0); - - // Use the factor to calculate the Jacobians - Matrix H1Actual, H2Actual; - factor.evaluateError(pose, point, H1Actual, H2Actual); - - // Use numerical derivatives to calculate the Jacobians - Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); - - // Verify the Jacobians are correct - CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); - CHECK(assert_equal(H2Expected, H2Actual, 1e-9)); -} - -/* ************************************************************************* - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp b/gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp deleted file mode 100644 index d9b94f1cb..000000000 --- a/gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testRangeFactor.cpp - * @brief Unit tests for DroneDynamicsVelXYFactor Class - * @author Duy-Nguyen Ta - * @date Oct 2014 - */ - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -// Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(3)); - -/* ************************************************************************* */ -LieVector factorError(const Pose3& pose, const LieVector& vel, const LieVector& coeffs, const DroneDynamicsVelXYFactor& factor) { - return factor.evaluateError(pose, vel, coeffs); -} - -/* ************************************************************************* */ -TEST( DroneDynamicsVelXYFactor, Error) { - // Create a factor - Key poseKey(1); - Key velKey(2); - Key coeffsKey(3); - Vector motors = (Vector(4) << 179, 180, 167, 168)/256.0; - Vector3 acc = (Vector(3) << 2., 1., 3.); - DroneDynamicsVelXYFactor factor(poseKey, velKey, coeffsKey, motors, acc, model); - - // Set the linearization point - Pose3 pose(Rot3::ypr(1.0, 2.0, 0.57), Point3()); - LieVector vel((Vector(3) << - -2.913425624770731, - -2.200086236883632, - -9.429823523226959)); - LieVector coeffs((Vector(4) << -9.3, 2.7, -6.5, 1.2)); - - - // Use the factor to calculate the error - Matrix H1, H2, H3; - Vector actualError(factor.evaluateError(pose, vel, coeffs, H1, H2, H3)); - - Vector expectedError = zero(3); - - // Verify we get the expected error -// CHECK(assert_equal(expectedError, actualError, 1e-9)); - - - // Use numerical derivatives to calculate the Jacobians - Matrix H1Expected, H2Expected, H3Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError, _1, vel, coeffs, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError, pose, _1, coeffs, factor), vel); - H3Expected = numericalDerivative11(boost::bind(&factorError, pose, vel, _1, factor), coeffs); - - // Verify the Jacobians are correct - CHECK(assert_equal(H1Expected, H1, 1e-9)); - CHECK(assert_equal(H2Expected, H2, 1e-9)); - CHECK(assert_equal(H3Expected, H3, 1e-9)); -} - -/* ************************************************************************* -TEST( DroneDynamicsVelXYFactor, Jacobian2D ) { - // Create a factor - Key poseKey(1); - Key pointKey(2); - double measurement(10.0); - RangeFactor2D factor(poseKey, pointKey, measurement, model); - - // Set the linearization point - Pose2 pose(1.0, 2.0, 0.57); - Point2 point(-4.0, 11.0); - - // Use the factor to calculate the Jacobians - Matrix H1Actual, H2Actual; - factor.evaluateError(pose, point, H1Actual, H2Actual); - - // Use numerical derivatives to calculate the Jacobians - Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); - - // Verify the Jacobians are correct - CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); - CHECK(assert_equal(H2Expected, H2Actual, 1e-9)); -} - -/* ************************************************************************* - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ -