diff --git a/gtsam/slam/DroneDynamicsFactor.h b/gtsam/slam/DroneDynamicsFactor.h new file mode 100644 index 000000000..ce305838a --- /dev/null +++ b/gtsam/slam/DroneDynamicsFactor.h @@ -0,0 +1,114 @@ + + +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * DroneDynamicsFactor.h + * + * Created on: Oct 1, 2014 + * Author: krunal + */ +#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 new file mode 100644 index 000000000..1268c1ac9 --- /dev/null +++ b/gtsam/slam/DroneDynamicsVelXYFactor.h @@ -0,0 +1,124 @@ + + +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * DroneDynamicsVelXYFactor.h + * + * Created on: Oct 1, 2014 + * Author: krunal + */ +#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 sqrtSumMotors = sqrt(motors(0)) + sqrt(motors(1)) + sqrt(motors(2)) + sqrt(motors(3)); + M(0,0) = sqrtSumMotors*acc(0); M(0, 1) = 1.0; + M(1,2) = 1.0; M(1, 3) = sqrtSumMotors*acc(1); + 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/testDroneDynamicsFactor.cpp b/gtsam/slam/tests/testDroneDynamicsFactor.cpp new file mode 100644 index 000000000..14004da3b --- /dev/null +++ b/gtsam/slam/tests/testDroneDynamicsFactor.cpp @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 Stephen Williams + * @date Oct 2012 + */ + +#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 new file mode 100644 index 000000000..e0bb1356d --- /dev/null +++ b/gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * 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 Stephen Williams + * @date Oct 2012 + */ + +#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); } +/* ************************************************************************* */ +