diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h new file mode 100644 index 000000000..24d866829 --- /dev/null +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -0,0 +1,62 @@ +/** + * @file VelocityConstraint3.h + * @brief A simple 3-way factor constraining LieScalar poses and velocity + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class VelocityConstraint3 : public NoiseModelFactor3 { +public: + +protected: + typedef NoiseModelFactor3 Base; + + /** default constructor to allow for serialization */ + VelocityConstraint3() {} + + double dt_; + +public: + + typedef boost::shared_ptr shared_ptr; + + ///TODO: comment + VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0) + : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {} + virtual ~VelocityConstraint3() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); } + + /** g(x) with optional derivative2 */ + Vector evaluateError(const LieScalar& x1, const LieScalar& x2, const LieScalar& v, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { + const size_t p = LieScalar::Dim(); + if (H1) *H1 = eye(p); + if (H2) *H2 = -eye(p); + if (H3) *H3 = eye(p)*dt_; + return x2.localCoordinates(x1.compose(LieScalar(v*dt_))); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); + } +}; // \VelocityConstraint3 + +} diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp new file mode 100644 index 000000000..21701fda4 --- /dev/null +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp @@ -0,0 +1,31 @@ +/** + * @file testVelocityConstraint3 + * @author Duy-Nguyen Ta + */ + +#include + +#include + +/* ************************************************************************* */ +using namespace gtsam; + +const double tol=1e-5; +const double dt = 1.0; + +LieScalar origin, + x1(1.0), x2(2.0), v(1.0); + +/* ************************************************************************* */ +TEST( testVelocityConstraint3, evaluateError) { + // hard constraints don't need a noise model + VelocityConstraint3 constraint(x1, x2, v, dt); + + // verify error function + EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol)); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */