diff --git a/.cproject b/.cproject index 5719b0da0..ff704cea8 100644 --- a/.cproject +++ b/.cproject @@ -526,6 +526,14 @@ true true + + make + -j4 + testEvent.run + true + true + true + make -j2 @@ -840,6 +848,14 @@ true true + + make + -j4 + testTOAFactor.run + true + true + true + make -j5 diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index e9ccbb2cb..92f94c3f3 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -19,7 +19,7 @@ // The two new headers that allow using our Automatic Differentiation Expression framework #include -#include +#include // Header order is close to far #include @@ -35,28 +35,26 @@ using namespace gtsam; int main(int argc, char** argv) { // 1. Create a factor graph container and add factors to it - NonlinearFactorGraph graph; + ExpressionFactorGraph graph; // Create Expressions for unknowns Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); // 2a. Add a prior on the first pose, setting it to the origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.push_back(ExpressionFactor(priorNoise, Pose2(0, 0, 0), x1)); + graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - BinaryExpression::Function f = traits::Between; - Expression(traits::Between, x1, x2); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x4,x5))); + graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model); + graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x5,x2))); + graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index 0c750c81c..e9c9e920d 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -24,14 +24,11 @@ // The two new headers that allow using our Automatic Differentiation Expression framework #include -#include +#include // Header order is close to far #include -#include -#include #include -#include #include #include #include @@ -40,27 +37,29 @@ using namespace std; using namespace gtsam; +using namespace noiseModel; /* ************************************************************************* */ int main(int argc, char* argv[]) { Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + Isotropic::shared_ptr measurementNoise = Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks and poses vector points = createPoints(); vector poses = createPoses(); // Create a factor graph - NonlinearFactorGraph graph; + ExpressionFactorGraph graph; // Specify uncertainty on first pose prior - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + Vector6 sigmas; sigmas << Vector3(0.3,0.3,0.3), Vector3(0.1,0.1,0.1); + Diagonal::shared_ptr poseNoise = Diagonal::Sigmas(sigmas); // Here we don't use a PriorFactor but directly the ExpressionFactor class - // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0] + // x0 is an Expression, and we create a factor wanting it to be equal to poses[0] Pose3_ x0('x',0); - graph.push_back(ExpressionFactor(poseNoise, poses[0], x0)); + graph.addExpressionFactor(x0, poses[0], poseNoise); // We create a constant Expression for the calibration here Cal3_S2_ cK(K); @@ -74,25 +73,26 @@ int main(int argc, char* argv[]) { // Below an expression for the prediction of the measurement: Point3_ p('l', j); Point2_ prediction = uncalibrate(cK, project(transform_to(x, p))); - // Again, here we use a ExpressionFactor - graph.push_back(ExpressionFactor(measurementNoise, measurement, prediction)); + // Again, here we use an ExpressionFactor + graph.addExpressionFactor(prediction, measurement, measurementNoise); } } // Add prior on first point to constrain scale, again with ExpressionFactor - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(ExpressionFactor(pointNoise, points[0], Point3_('l', 0))); + Isotropic::shared_ptr pointNoise = Isotropic::Sigma(3, 0.1); + graph.addExpressionFactor(Point3_('l', 0), points[0], pointNoise); // Create perturbed initial - Values initialEstimate; + Values initial; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); - cout << "initial error = " << graph.error(initialEstimate) << endl; + initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + cout << "initial error = " << graph.error(initial) << endl; /* Optimize the graph and print results */ - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + Values result = DoglegOptimizer(graph, initial).optimize(); cout << "final error = " << graph.error(result) << endl; return 0; diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 9c410ac25..bf2056cc8 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -110,7 +110,7 @@ struct LieGroup { Class retract(const TangentVector& v, // ChartJacobian H1, ChartJacobian H2 = boost::none) const { Jacobian D_g_v; - Class g = Class::ChartAtOrigin::Retract(v,D_g_v); + Class g = Class::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); Class h = compose(g,H1,H2); if (H2) *H2 = (*H2) * D_g_v; return h; @@ -120,7 +120,7 @@ struct LieGroup { ChartJacobian H1, ChartJacobian H2 = boost::none) const { Class h = between(g,H1,H2); Jacobian D_v_h; - TangentVector v = Class::ChartAtOrigin::Local(h, D_v_h); + TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); if (H1) *H1 = D_v_h * (*H1); if (H2) *H2 = D_v_h * (*H2); return v; diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index e5cfbdd3b..a7dc37d55 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -296,6 +296,48 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* x2, x3, delta); } +/** + * Compute numerical derivative in argument 1 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative41( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3, x4), x1, delta); +} + +/** + * Compute numerical derivative in argument 2 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative42( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3, x4), x2, delta); +} + /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * function. This is implemented simply as the derivative of the gradient. diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h index 4bbca85ca..791d5c04d 100644 --- a/gtsam/base/testLie.h +++ b/gtsam/base/testLie.h @@ -34,23 +34,25 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_, Matrix H1, H2; typedef traits T; + typedef OptionalJacobian OJ; // Inverse + OJ none; EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); - EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1)); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t1, none),H1)); EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); - EXPECT(assert_equal(numericalDerivative11(T::Inverse, t2),H1)); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t2, none),H1)); // Compose EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Compose, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Compose, t1, t2, none, none), H2)); // Between EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Between, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Between, t1, t2, none, none), H2)); } // Do a comprehensive test of Lie Group Chart derivatives template @@ -59,17 +61,20 @@ void testChartDerivatives(TestResult& result_, const std::string& name_, Matrix H1, H2; typedef traits T; + typedef typename G::TangentVector V; + typedef OptionalJacobian OJ; // Retract - typename G::TangentVector w12 = T::Local(t1, t2); + OJ none; + V w12 = T::Local(t1, t2); EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Retract, t1, w12, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Retract, t1, w12, none, none), H2)); // Local EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Local, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Local, t1, t2, none, none), H2)); } } // namespace gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 01a6d6052..6f4f93cf9 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -165,12 +165,12 @@ public: /// @{ inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} inline void operator *= (double s) {x_*=s;y_*=s;} - Point2 inverse() { return -(*this);} - Point2 compose(const Point2& q) { return (*this)+q;} - Point2 between(const Point2& q) { return q-(*this);} - Vector2 localCoordinates(const Point2& q) { return between(q).vector();} - Point2 retract(const Vector2& v) {return compose(Point2(v));} - static Vector2 Logmap(const Point2& p) {return p.vector();} + Point2 inverse() const { return -(*this);} + Point2 compose(const Point2& q) const { return (*this)+q;} + Point2 between(const Point2& q) const { return q-(*this);} + Vector2 localCoordinates(const Point2& q) const { return between(q).vector();} + Point2 retract(const Vector2& v) const { return compose(Point2(v));} + static Vector2 Logmap(const Point2& p) { return p.vector();} static Point2 Expmap(const Vector2& v) { return Point2(v);} /// @} diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 5b133dc60..c9dee9003 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -164,12 +164,12 @@ namespace gtsam { /// @name Deprecated /// @{ - Point3 inverse() { return -(*this);} - Point3 compose(const Point3& q) { return (*this)+q;} - Point3 between(const Point3& q) { return q-(*this);} - Vector3 localCoordinates(const Point3& q) { return between(q).vector();} - Point3 retract(const Vector3& v) {return compose(Point3(v));} - static Vector3 Logmap(const Point3& p) {return p.vector();} + Point3 inverse() const { return -(*this);} + Point3 compose(const Point3& q) const { return (*this)+q;} + Point3 between(const Point3& q) const { return q-(*this);} + Vector3 localCoordinates(const Point3& q) const { return between(q).vector();} + Point3 retract(const Vector3& v) const { return compose(Point3(v));} + static Vector3 Logmap(const Point3& p) { return p.vector();} static Point3 Expmap(const Vector3& v) { return Point3(v);} /// @} diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 3d8e41af5..0fe699116 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -55,46 +55,6 @@ TEST(SO3 , Retract) { EXPECT(actual.isApprox(R2)); } -//****************************************************************************** -TEST(SO3 , Compose) { - SO3 expected = R1 * R2; - Matrix actualH1, actualH2; - SO3 actual = traits::Compose(R1, R2, actualH1, actualH2); - EXPECT(traits::Equals(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(traits::Compose, R1, R2); - EXPECT(assert_equal(numericalH1,actualH1)); - - Matrix numericalH2 = numericalDerivative22(traits::Compose, R1, R2); - EXPECT(assert_equal(numericalH2,actualH2)); -} - -//****************************************************************************** -TEST(SO3 , Between) { - SO3 expected = R1.inverse() * R2; - Matrix actualH1, actualH2; - SO3 actual = traits::Between(R1, R2, actualH1, actualH2); - EXPECT(traits::Equals(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(traits::Between, R1, R2); - EXPECT(assert_equal(numericalH1,actualH1)); - - Matrix numericalH2 = numericalDerivative22(traits::Between, R1, R2); - EXPECT(assert_equal(numericalH2,actualH2)); -} - -//****************************************************************************** -TEST(SO3 , Inverse) { - SO3 expected(Eigen::AngleAxisd(-0.1, z_axis)); - - Matrix actualH; - SO3 actual = traits::Inverse(R1, actualH); - EXPECT(traits::Equals(expected,actual)); - - Matrix numericalH = numericalDerivative11(traits::Inverse, R1); - EXPECT(assert_equal(numericalH,actualH)); -} - //****************************************************************************** TEST(SO3 , Invariants) { check_group_invariants(id,id); diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 0b3dea645..a86e7312a 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -32,6 +32,7 @@ #include namespace MPL = boost::mpl::placeholders; +#include // operator typeid #include class ExpressionFactorBinaryTest; @@ -245,6 +246,15 @@ public: virtual ~ExpressionNode() { } + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, + const ExpressionNode& node) { + os << "Expression of type " << typeid(T).name(); + if (node.traceSize_>0) os << ", trace size = " << node.traceSize_; + os << "\n"; + return os; + } + /// Return keys that play in this expression as a set virtual std::set keys() const { std::set keys; @@ -621,10 +631,11 @@ struct FunctionalNode { template class UnaryExpression: public FunctionalNode >::type { + typedef typename MakeOptionalJacobian::type OJ1; + public: - typedef boost::function< - T(const A1&, typename MakeOptionalJacobian::type)> Function; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -664,13 +675,15 @@ public: /// Binary Expression template -class BinaryExpression: public FunctionalNode >::type { +class BinaryExpression: + public FunctionalNode >::type { + + typedef typename MakeOptionalJacobian::type OJ1; + typedef typename MakeOptionalJacobian::type OJ2; public: - typedef boost::function< - T(const A1&, const A2&, typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type)> Function; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -718,15 +731,16 @@ public: /// Ternary Expression template -class TernaryExpression: public FunctionalNode >::type { +class TernaryExpression: + public FunctionalNode >::type { + + typedef typename MakeOptionalJacobian::type OJ1; + typedef typename MakeOptionalJacobian::type OJ2; + typedef typename MakeOptionalJacobian::type OJ3; public: - typedef boost::function< - T(const A1&, const A2&, const A3&, - typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type)> Function; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 2e0b6c01e..3c9c4eeee 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -52,6 +52,11 @@ private: public: + /// Print + void print(const std::string& s) const { + std::cout << s << *root_ << std::endl; + } + // Construct a constant expression Expression(const T& value) : root_(new ConstantExpression(value)) { @@ -104,6 +109,20 @@ public: root_(new BinaryExpression(function, expression1, expression2)) { } + /// Construct a binary method expression + template + Expression(const Expression& expression1, + T (A1::*method)(const A2&, const A3&, + typename TernaryExpression::OJ1, + typename TernaryExpression::OJ2, + typename TernaryExpression::OJ3) const, + const Expression& expression2, const Expression& expression3) : + root_( + new TernaryExpression( + boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, + expression2, expression3)) { + } + /// Construct a ternary function expression template Expression(typename TernaryExpression::Function function, diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index e75add618..4769e5048 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -109,5 +109,5 @@ public: }; // ExpressionFactor -} +} // \ namespace gtsam diff --git a/gtsam/nonlinear/ExpressionFactorGraph.h b/gtsam/nonlinear/ExpressionFactorGraph.h new file mode 100644 index 000000000..122bd429f --- /dev/null +++ b/gtsam/nonlinear/ExpressionFactorGraph.h @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExpressionFactorGraph.h + * @brief Factor graph that supports adding ExpressionFactors directly + * @author Frank Dellaert + * @date December 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Factor graph that supports adding ExpressionFactors directly + */ +class ExpressionFactorGraph: public NonlinearFactorGraph { + +public: + + /// @name Adding Factors + /// @{ + + /** + * Directly add ExpressionFactor that implements |h(x)-z|^2_R + * @param h expression that implements measurement function + * @param z measurement + * @param R model + */ + template + void addExpressionFactor(const Expression& h, const T& z, + const SharedNoiseModel& R) { + push_back(boost::make_shared >(R, z, h)); + } + + /// @} +}; + +} diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 1ea6236e8..80100af5e 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include @@ -78,9 +77,6 @@ namespace unary { Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { return Point2(); } -LieScalar f1(const Point3& p, OptionalJacobian<1, 3> H) { - return LieScalar(0.0); -} double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } @@ -92,11 +88,6 @@ TEST(Expression, Unary0) { Expression e(f0, p); EXPECT(expected == e.keys()); } -TEST(Expression, Unary1) { - using namespace unary; - Expression e(f1, p); - EXPECT(expected == e.keys()); -} TEST(Expression, Unary2) { using namespace unary; Expression e(f2, p); diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h new file mode 100644 index 000000000..9d35331bb --- /dev/null +++ b/gtsam_unstable/geometry/Event.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 Event + * @brief Space-time event + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/// A space-time event +class Event { + + double time_; ///< Time event was generated + Point3 location_; ///< Location at time event was generated + +public: + enum { dimension = 4 }; + + /// Speed of sound + static const double Speed; + static const Matrix14 JacobianZ; + + /// Default Constructor + Event() : + time_(0) { + } + + /// Constructor from time and location + Event(double t, const Point3& p) : + time_(t), location_(p) { + } + + /// Constructor with doubles + Event(double t, double x, double y, double z) : + time_(t), location_(x, y, z) { + } + + double time() const { return time_;} + Point3 location() const { return location_;} + + // TODO we really have to think of a better way to do linear arguments + double height(OptionalJacobian<1,4> H = boost::none) const { + if (H) *H = JacobianZ; + return location_.z(); + } + + /** print with optional string */ + void print(const std::string& s = "") const { + std::cout << s << "time = " << time_; + location_.print("; location"); + } + + /** equals with an tolerance */ + bool equals(const Event& other, double tol = 1e-9) const { + return std::abs(time_ - other.time_) < tol + && location_.equals(other.location_, tol); + } + + /// Updates a with tangent space delta + inline Event retract(const Vector4& v) const { + return Event(time_ + v[0], location_.retract(v.tail(3))); + } + + /// Returns inverse retraction + inline Vector4 localCoordinates(const Event& q) const { + return Vector4::Zero(); // TODO + } + + /// Time of arrival to given microphone + double toa(const Point3& microphone, // + OptionalJacobian<1, 4> H1 = boost::none, // + OptionalJacobian<1, 3> H2 = boost::none) const { + Matrix13 D1, D2; + double distance = location_.distance(microphone, D1, D2); + if (H1) + // derivative of toa with respect to event + *H1 << 1.0, D1 / Speed; + if (H2) + // derivative of toa with respect to microphone location + *H2 << D2 / Speed; + return time_ + distance / Speed; + } +}; + +const double Event::Speed = 330; +const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); + +// Define GTSAM traits +template<> +struct GTSAM_EXPORT traits : internal::Manifold {}; + +} //\ namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp new file mode 100644 index 000000000..433ca7e7f --- /dev/null +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 testEvent.cpp + * @brief Unit tests for space time "Event" + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Create a noise model for the TOA error +static const double ms = 1e-3; +static const double cm = 1e-2; +typedef Eigen::Matrix Vector1; +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms)); + +static const double timeOfEvent = 25; +static const Event exampleEvent(timeOfEvent, 1, 0, 0); +static const Point3 microphoneAt0; + +//***************************************************************************** +TEST( Event, Constructor ) { + const double t = 0; + Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); +} + +//***************************************************************************** +TEST( Event, Toa1 ) { + Event event(0, 1, 0, 0); + double expected = 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); +} + +//***************************************************************************** +TEST( Event, Toa2 ) { + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); +} + +//************************************************************************* +TEST (Event, Derivatives) { + Matrix14 actualH1; + Matrix13 actualH2; + exampleEvent.toa(microphoneAt0, actualH1, actualH2); + Matrix expectedH1 = numericalDerivative11( + boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none), + exampleEvent); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none), + microphoneAt0); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); +} + +//***************************************************************************** +TEST( Event, Expression ) { + Key key = 12; + Expression event_(key); + Expression knownMicrophone_(microphoneAt0); // constant expression + Expression expression(&Event::toa, event_, knownMicrophone_); + + Values values; + values.insert(key, exampleEvent); + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); +} + +//***************************************************************************** +TEST(Event, Retract) { + Event event, expected(1, 2, 3, 4); + Vector4 v; + v << 1, 2, 3, 4; + EXPECT(assert_equal(expected, event.retract(v))); +} + +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** + diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h new file mode 100644 index 000000000..b500b36e3 --- /dev/null +++ b/gtsam_unstable/slam/TOAFactor.h @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * 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 TOAFactor.h + * @brief "Time of Arrival" factor + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include + +namespace gtsam { + +/// A "Time of Arrival" factor - so little code seems hardly worth it :-) +class TOAFactor: public ExpressionFactor { + + typedef Expression double_; + +public: + + /** + * Constructor + * @param some expression yielding an event + * @param microphone_ expression yielding a microphone location + * @param toaMeasurement time of arrival at microphone + * @param model noise model + */ + TOAFactor(const Expression& eventExpression, + const Expression& microphone_, double toaMeasurement, + const SharedNoiseModel& model) : + ExpressionFactor(model, toaMeasurement, + double_(&Event::toa, eventExpression, microphone_)) { + } + +}; + +} //\ namespace gtsam + diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp new file mode 100644 index 000000000..879f7095e --- /dev/null +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTOAFactor.cpp + * @brief Unit tests for "Time of Arrival" factor + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// typedefs +typedef Eigen::Matrix Vector1; +typedef Expression Double_; +typedef Expression Point3_; +typedef Expression Event_; + +// units +static const double ms = 1e-3; +static const double cm = 1e-2; + +// Create a noise model for the TOA error +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); + +static const double timeOfEvent = 25; +static const Event exampleEvent(timeOfEvent, 1, 0, 0); +static const Point3 microphoneAt0; + +//***************************************************************************** +TEST( TOAFactor, NewWay ) { + Key key = 12; + Event_ eventExpression(key); + Point3_ microphoneConstant(microphoneAt0); // constant expression + double measurement = 7; + Double_ expression(&Event::toa, eventExpression, microphoneConstant); + ExpressionFactor factor(model, measurement, expression); +} + +//***************************************************************************** +TEST( TOAFactor, WholeEnchilada ) { + + static const bool verbose = false; + + // Create microphones + const double height = 0.5; + vector microphones; + microphones.push_back(Point3(0, 0, height)); + microphones.push_back(Point3(403 * cm, 0, height)); + microphones.push_back(Point3(403 * cm, 403 * cm, height)); + microphones.push_back(Point3(0, 403 * cm, 2 * height)); + EXPECT_LONGS_EQUAL(4, microphones.size()); +// microphones.push_back(Point3(200 * cm, 200 * cm, height)); + + // Create a ground truth point + const double timeOfEvent = 0; + Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm); + + // Simulate simulatedTOA + size_t K = microphones.size(); + vector simulatedTOA(K); + for (size_t i = 0; i < K; i++) { + simulatedTOA[i] = groundTruthEvent.toa(microphones[i]); + if (verbose) { + cout << "mic" << i << " = " << microphones[i] << endl; + cout << "z" << i << " = " << simulatedTOA[i] / ms << endl; + } + } + + // Now, estimate using non-linear optimization + ExpressionFactorGraph graph; + Key key = 12; + Event_ eventExpression(key); + for (size_t i = 0; i < K; i++) { + Point3_ microphone_i(microphones[i]); // constant expression + Double_ predictTOA(&Event::toa, eventExpression, microphone_i); + graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); + } + + /// Print the graph + if (verbose) + GTSAM_PRINT(graph); + + // Create initial estimate + Values initialEstimate; + //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); + Vector4 delta; + delta << 0.1, 0.1, -0.1, 0.1; + Event estimatedEvent = groundTruthEvent.retract(delta); + initialEstimate.insert(key, estimatedEvent); + + // Print + if (verbose) + initialEstimate.print("Initial Estimate:\n"); + + // Optimize using Levenberg-Marquardt optimization. + LevenbergMarquardtParams params; + params.setAbsoluteErrorTol(1e-10); + if (verbose) + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + if (verbose) + result.print("Final Result:\n"); + + EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); +} +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** + diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m index 05483e589..2900aed99 100644 --- a/matlab/+gtsam/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -33,12 +33,12 @@ for i = 0:keys.size-1 end gtsam.plotPose3(lastPose, P, scale); catch err - warning(['no Pose3 at ' lastKey]); + % warning(['no Pose3 at ' lastKey]); end - lastIndex = i; end + lastIndex = i; catch - warning(['no Pose3 at ' key]); + % warning(['no Pose3 at ' key]); end % Draw final pose @@ -53,7 +53,7 @@ for i = 0:keys.size-1 end gtsam.plotPose3(lastPose, P, scale); catch - warning(['no Pose3 at ' lastIndex]); + % warning(['no Pose3 at ' lastIndex]); end end