12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in feature/SoundSlam (pull request #85) 

Sound Expressions, including ExpressionFactorGraph...
release/4.3a0
Frank Dellaert 2014-12-28 21:34:07 +01:00
parent ba6d088fad 9367dc2fb5
commit 478f354922
19 changed files with 606 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -526,6 +526,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEvent.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testEvent.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -840,6 +848,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTOAFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testTOAFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

18
examples/Pose2SLAMExampleExpressions.cpp
View File

@ -19,7 +19,7 @@
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -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<Pose2>(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<Pose2, Pose2, Pose2>::Function f = traits<Pose2>::Between;
Expression<Pose2>(traits<Pose2>::Between, x1, x2);
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, 0 ), between(x1,x2)));
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x2,x3)));
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x3,x4)));
graph.push_back(ExpressionFactor<Pose2>(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<Pose2>(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

36
examples/SFMExampleExpressions.cpp
View File

@ -24,14 +24,11 @@
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <examples/SFMdata.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
@ -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<Point3> points = createPoints();
vector<Pose3> 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<Pose3>(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<Point2>(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<Point3>(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;

4
gtsam/base/Lie.h
View File

@ -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;

42
gtsam/base/numericalDerivative.h
View File

@ -296,6 +296,48 @@ inline typename internal::FixedSizeMatrix<Y,X3>::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<class Y, class X1, class X2, class X3, class X4>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(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<class Y, class X1, class X2, class X3, class X4>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2>(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.

27
gtsam/base/testLie.h
View File

@ -34,23 +34,25 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
Matrix H1, H2;
typedef traits<G> T;
typedef OptionalJacobian<T::dimension,T::dimension> 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<G,G,OJ>(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<G,G,OJ>(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<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(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<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
}
// Do a comprehensive test of Lie Group Chart derivatives
template<typename G>
@ -59,17 +61,20 @@ void testChartDerivatives(TestResult& result_, const std::string& name_,
Matrix H1, H2;
typedef traits<G> T;
typedef typename G::TangentVector V;
typedef OptionalJacobian<T::dimension,T::dimension> 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<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(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<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
}
} // namespace gtsam

12
gtsam/geometry/Point2.h
View File

@ -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);}
/// @}

12
gtsam/geometry/Point3.h
View File

@ -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);}
/// @}

40
gtsam/geometry/tests/testSO3.cpp
View File

@ -55,46 +55,6 @@ TEST(SO3 , Retract) {
EXPECT(actual.isApprox(R2));
}
//******************************************************************************
TEST(SO3 , Compose) {
SO3 expected = R1 * R2;
Matrix actualH1, actualH2;
SO3 actual = traits<SO3>::Compose(R1, R2, actualH1, actualH2);
EXPECT(traits<SO3>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<SO3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<SO3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2));
}
//******************************************************************************
TEST(SO3 , Between) {
SO3 expected = R1.inverse() * R2;
Matrix actualH1, actualH2;
SO3 actual = traits<SO3>::Between(R1, R2, actualH1, actualH2);
EXPECT(traits<SO3>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<SO3>::Between, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<SO3>::Between, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2));
}
//******************************************************************************
TEST(SO3 , Inverse) {
SO3 expected(Eigen::AngleAxisd(-0.1, z_axis));
Matrix actualH;
SO3 actual = traits<SO3>::Inverse(R1, actualH);
EXPECT(traits<SO3>::Equals(expected,actual));
Matrix numericalH = numericalDerivative11(traits<SO3>::Inverse, R1);
EXPECT(assert_equal(numericalH,actualH));
}
//******************************************************************************
TEST(SO3 , Invariants) {
check_group_invariants(id,id);

38
gtsam/nonlinear/Expression-inl.h
View File

@ -32,6 +32,7 @@
#include <boost/mpl/fold.hpp>
namespace MPL = boost::mpl::placeholders;
#include <typeinfo> // operator typeid
#include <map>
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<Key> keys() const {
std::set<Key> keys;
@ -621,10 +631,11 @@ struct FunctionalNode {
template<class T, class A1>
class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
public:
typedef boost::function<
T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> Function;
typedef boost::function<T(const A1&, OJ1)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
typedef typename Base::Record Record;
@ -664,13 +675,15 @@ public:
/// Binary Expression
template<class T, class A1, class A2>
class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
class BinaryExpression:
public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
public:
typedef boost::function<
T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type)> Function;
typedef boost::function<T(const A1&, const A2&, OJ1, OJ2)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
typedef typename Base::Record Record;
@ -718,15 +731,16 @@ public:
/// Ternary Expression
template<class T, class A1, class A2, class A3>
class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
class TernaryExpression:
public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
typedef typename MakeOptionalJacobian<T, A3>::type OJ3;
public:
typedef boost::function<
T(const A1&, const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type)> Function;
typedef boost::function<T(const A1&, const A2&, const A3&, OJ1, OJ2, OJ3)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
typedef typename Base::Record Record;

19
gtsam/nonlinear/Expression.h
View File

@ -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<T>(value)) {
@ -104,6 +109,20 @@ public:
root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
}
/// Construct a binary method expression
template<typename A1, typename A2, typename A3>
Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, const A3&,
typename TernaryExpression<T, A1, A2, A3>::OJ1,
typename TernaryExpression<T, A1, A2, A3>::OJ2,
typename TernaryExpression<T, A1, A2, A3>::OJ3) const,
const Expression<A2>& expression2, const Expression<A3>& expression3) :
root_(
new TernaryExpression<T, A1, A2, A3>(
boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
expression2, expression3)) {
}
/// Construct a ternary function expression
template<typename A1, typename A2, typename A3>
Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,

2
gtsam/nonlinear/ExpressionFactor.h
View File

@ -109,5 +109,5 @@ public:
};
// ExpressionFactor
}
} // \ namespace gtsam

51
gtsam/nonlinear/ExpressionFactorGraph.h Normal file
View File

@ -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 <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
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<typename T>
void addExpressionFactor(const Expression<T>& h, const T& z,
const SharedNoiseModel& R) {
push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
}
/// @}
};
}

9
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieScalar.h>
#include <CppUnitLite/TestHarness.h>
@ -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<Point2> e(f0, p);
EXPECT(expected == e.keys());
}
TEST(Expression, Unary1) {
using namespace unary;
Expression<double> e(f1, p);
EXPECT(expected == e.keys());
}
TEST(Expression, Unary2) {
using namespace unary;
Expression<double> e(f2, p);

109
gtsam_unstable/geometry/Event.h Normal file
View File

@ -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 <gtsam/geometry/Point3.h>
#include <cmath>
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<Event> : internal::Manifold<Event> {};
} //\ namespace gtsam

100
gtsam_unstable/geometry/tests/testEvent.cpp Normal file
View File

@ -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 <gtsam_unstable/geometry/Event.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/Expression.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
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<double, 1, 1> 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<double, Event>(
boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none),
exampleEvent);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
Matrix expectedH2 = numericalDerivative11<double, Point3>(
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> event_(key);
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
Expression<double> 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);
}
//*****************************************************************************

49
gtsam_unstable/slam/TOAFactor.h Normal file
View File

@ -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 <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam_unstable/geometry/Event.h>
namespace gtsam {
/// A "Time of Arrival" factor - so little code seems hardly worth it :-)
class TOAFactor: public ExpressionFactor<double> {
typedef Expression<double> 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<Event>& eventExpression,
const Expression<Point3>& microphone_, double toaMeasurement,
const SharedNoiseModel& model) :
ExpressionFactor<double>(model, toaMeasurement,
double_(&Event::toa, eventExpression, microphone_)) {
}
};
} //\ namespace gtsam

133
gtsam_unstable/slam/tests/testTOAFactor.cpp Normal file
View File

@ -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 <gtsam_unstable/geometry/Event.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/format.hpp>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;
// typedefs
typedef Eigen::Matrix<double, 1, 1> Vector1;
typedef Expression<double> Double_;
typedef Expression<Point3> Point3_;
typedef Expression<Event> 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<double> factor(model, measurement, expression);
}
//*****************************************************************************
TEST( TOAFactor, WholeEnchilada ) {
static const bool verbose = false;
// Create microphones
const double height = 0.5;
vector<Point3> 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<double> 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<Event>(key), 1e-6));
}
//*****************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//*****************************************************************************

8
matlab/+gtsam/plot3DTrajectory.m
View File

@ -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