diff --git a/cpp/BetweenFactor.h b/cpp/BetweenFactor.h index 99e68f414..3b6d22109 100644 --- a/cpp/BetweenFactor.h +++ b/cpp/BetweenFactor.h @@ -65,8 +65,11 @@ namespace gtsam { // h - z T hx = between(p1, p2); // TODO should be done by noise model - if (H1) *H1 = square_root_inverse_covariance_ * Dbetween1(p1, p2); - if (H2) *H2 = square_root_inverse_covariance_ * Dbetween2(p1, p2); + if (H1 || H2) { + between(p1,p2,H1,H2); + if (H1) *H1 = square_root_inverse_covariance_ * *H1; + if (H2) *H2 = square_root_inverse_covariance_ * *H2; + } // manifold equivalent of h(x)-z -> log(z,h(x)) // TODO use noise model, error vector is not whitened yet return square_root_inverse_covariance_ * logmap(measured_, hx); diff --git a/cpp/Factor.h b/cpp/Factor.h index 4d4fa512b..8576bf259 100644 --- a/cpp/Factor.h +++ b/cpp/Factor.h @@ -36,7 +36,7 @@ namespace gtsam { * provide the appropriate values at the appropriate time. */ template - class Factor : boost::noncopyable, public Testable< Factor > + class Factor : public Testable< Factor > { public: diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 338e35b15..f4fc93aa2 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -24,6 +24,12 @@ #include "FactorGraph.h" #include "graph-inl.h" +#define INSTANTIATE_FACTOR_GRAPH(F) \ + template class FactorGraph; \ + /*template boost::shared_ptr removeAndCombineFactors(FactorGraph&, const std::string&);*/ \ + template FactorGraph combine(const FactorGraph&, const FactorGraph&); + + // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) @@ -271,10 +277,10 @@ void FactorGraph::associateFactor(int index, sharedFactor factor) { } /* ************************************************************************* */ -template template +template template PredecessorMap FactorGraph::findMinimumSpanningTree() const { - SDGraph g = gtsam::toBoostGraph, sharedFactor, Key>(*this); + SDGraph g = gtsam::toBoostGraph, Factor2, Key>(*this); // find minimum spanning tree vector::Vertex> p_map(boost::num_vertices(g)); @@ -285,8 +291,8 @@ PredecessorMap FactorGraph::findMinimumSpanningTree() const { typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; typename vector::Vertex>::iterator vi; for (vi = p_map.begin(); vi!=p_map.end(); itVertex++, vi++) { - string key = boost::get(boost::vertex_name, g, *itVertex); - string parent = boost::get(boost::vertex_name, g, *vi); + Key key = boost::get(boost::vertex_name, g, *itVertex); + Key parent = boost::get(boost::vertex_name, g, *vi); // printf("%s parent: %s\n", key.c_str(), parent.c_str()); tree.insert(key, parent); } @@ -294,8 +300,8 @@ PredecessorMap FactorGraph::findMinimumSpanningTree() const { return tree; } -template template -void FactorGraph::split(map tree, FactorGraph& Ab1, FactorGraph& Ab2) const { +template template +void FactorGraph::split(const PredecessorMap& tree, FactorGraph& Ab1, FactorGraph& Ab2) const{ BOOST_FOREACH(sharedFactor factor, factors_){ if (factor->keys().size() > 2) @@ -306,14 +312,17 @@ void FactorGraph::split(map tree, FactorGraph& Ab1, Fa continue; } - string key1 = factor->keys().front(); - string key2 = factor->keys().back(); + boost::shared_ptr factor2 = boost::dynamic_pointer_cast(factor); + if (!factor2) continue; + + Key key1 = factor2->key1(); + Key key2 = factor2->key2(); // if the tree contains the key - if (tree.find(key1) != tree.end() && tree[key1].compare(key2) == 0 || - tree.find(key2) != tree.end() && tree[key2].compare(key1) == 0) - Ab1.push_back(factor); + if (tree.find(key1) != tree.end() && tree.find(key1)->second.compare(key2) == 0 || + tree.find(key2) != tree.end() && tree.find(key2)->second.compare(key1) == 0) + Ab1.push_back(factor2); else - Ab2.push_back(factor); + Ab2.push_back(factor2); } } diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index af54edcbf..432814236 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -120,13 +120,13 @@ namespace gtsam { /** * find the minimum spanning tree. */ - template PredecessorMap findMinimumSpanningTree() const; + template PredecessorMap findMinimumSpanningTree() const; /** * Split the graph into two parts: one corresponds to the given spanning tre, * and the other corresponds to the rest of the factors */ - template void split(std::map tree, + template void split(const PredecessorMap& tree, FactorGraph& Ab1, FactorGraph& Ab2) const; private: diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index d8da80d7e..5a4a74396 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -27,7 +27,7 @@ namespace gtsam { * GaussianFactor is non-mutable (all methods const!). * The factor value is exp(-0.5*||Ax-b||^2) */ -class GaussianFactor: public Factor { +class GaussianFactor: boost::noncopyable, public Factor { public: typedef boost::shared_ptr shared_ptr; diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index 256735dde..2799d98b5 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -10,7 +10,7 @@ using namespace boost::assign; #include -#include "NonlinearFactorGraph.h" +#include "NonlinearFactorGraph-inl.h" #include "GaussianFactor.h" #include "VectorConfig.h" diff --git a/cpp/Key.h b/cpp/Key.h index 464ab7280..eb52eeeee 100644 --- a/cpp/Key.h +++ b/cpp/Key.h @@ -43,6 +43,7 @@ namespace gtsam { bool operator< (const Symbol& compare) const { return j_; + namespace gtsam { template size_t Lie::dim() const { diff --git a/cpp/LieConfig-inl.h b/cpp/LieConfig-inl.h index 7e19759e5..ff0407fae 100644 --- a/cpp/LieConfig-inl.h +++ b/cpp/LieConfig-inl.h @@ -5,8 +5,6 @@ * Author: richard */ -#include "LieConfig.h" - #include #include #include @@ -14,6 +12,15 @@ #include #include "VectorConfig.h" +#include "Lie-inl.h" + +#include "LieConfig.h" + +#define INSTANTIATE_LIE_CONFIG(J,T) \ + /*INSTANTIATE_LIE(T);*/ \ + template LieConfig expmap(const LieConfig&, const VectorConfig&); \ + template LieConfig expmap(const LieConfig&, const Vector&); \ + template class LieConfig; using namespace std; @@ -47,7 +54,7 @@ namespace gtsam { template void LieConfig::insert(const J& name, const T& val) { values_.insert(make_pair(name, val)); - dim_ += dim(val); + dim_ += gtsam::dim(val); } template diff --git a/cpp/LieConfig.h b/cpp/LieConfig.h index 3858204b8..f3445d0e5 100644 --- a/cpp/LieConfig.h +++ b/cpp/LieConfig.h @@ -13,30 +13,12 @@ #include "Vector.h" #include "Testable.h" -#include "Lie.h" -#include "Key.h" namespace boost { template class optional; } +namespace gtsam { class VectorConfig; } namespace gtsam { - class VectorConfig; - - // TODO: why are these defined *before* the class ? - template class LieConfig; - - /** Dimensionality of the tangent space */ - template - size_t dim(const LieConfig& c); - - /** Add a delta config */ - template - LieConfig expmap(const LieConfig& c, const VectorConfig& delta); - - /** Add a delta vector, uses iterator order */ - template - LieConfig expmap(const LieConfig& c, const Vector& delta); - /** * Lie type configuration */ @@ -63,7 +45,7 @@ namespace gtsam { LieConfig() : dim_(0) {} LieConfig(const LieConfig& config) : - values_(config.values_), dim_(dim(config)) {} + values_(config.values_), dim_(config.dim_) {} virtual ~LieConfig() {} /** print */ @@ -84,6 +66,11 @@ namespace gtsam { /** The number of variables in this config */ size_t size() const { return values_.size(); } + /** + * The dimensionality of the tangent space + */ + size_t dim() const { return dim_; } + const_iterator begin() const { return values_.begin(); } const_iterator end() const { return values_.end(); } iterator begin() { return values_.begin(); } @@ -105,7 +92,7 @@ namespace gtsam { /** Replace all keys and variables */ LieConfig& operator=(const LieConfig& rhs) { values_ = rhs.values_; - dim_ = dim(rhs); + dim_ = rhs.dim_; return (*this); } @@ -115,14 +102,18 @@ namespace gtsam { dim_ = 0; } -// friend LieConfig expmap(const LieConfig& c, const VectorConfig& delta); -// friend LieConfig expmap(const LieConfig& c, const Vector& delta); - friend size_t dim(const LieConfig& c); - }; /** Dimensionality of the tangent space */ template - size_t dim(const LieConfig& c) { return c.dim_; } + inline size_t dim(const LieConfig& c) { return c.dim(); } + + /** Add a delta config */ + template + LieConfig expmap(const LieConfig& c, const VectorConfig& delta); + + /** Add a delta vector, uses iterator order */ + template + LieConfig expmap(const LieConfig& c, const Vector& delta); } diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 686f45a86..dbdd08a89 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -200,11 +200,11 @@ testSimulated3D_LDADD = libgtsam.la check_PROGRAMS += testSimulated3D # Pose SLAM headers -headers += BetweenFactor.h LieConfig.h LieConfig-inl.h TupleConfig.h +headers += BetweenFactor.h LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h # 2D Pose constraints -headers += Pose2Factor.h Pose2Prior.h -sources += Pose2Config.cpp Pose2Graph.cpp +headers += Pose2Prior.h +sources += Pose2Graph.cpp check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph testPose2Factor_SOURCES = $(example) testPose2Factor.cpp testPose2Factor_LDADD = libgtsam.la @@ -213,14 +213,12 @@ testPose2Config_LDADD = libgtsam.la testPose2Graph_SOURCES = $(example) testPose2Graph.cpp testPose2Graph_LDADD = libgtsam.la -# 2D Bearing and Range -headers += BearingFactor.h RangeFactor.h -sources += -check_PROGRAMS += testBearingFactor testRangeFactor -testBearingFactor_SOURCES = $(example) testBearingFactor.cpp -testBearingFactor_LDADD = libgtsam.la -testRangeFactor_SOURCES = $(example) testRangeFactor.cpp -testRangeFactor_LDADD = libgtsam.la +# 2D SLAM using Bearing and Range +headers += +sources += planarSLAM.cpp +check_PROGRAMS += testPlanarSLAM +testPlanarSLAM_SOURCES = $(example) testPlanarSLAM.cpp +testPlanarSLAM_LDADD = libgtsam.la # 3D Pose constraints headers += Pose3Factor.h @@ -242,8 +240,7 @@ testSimpleCamera_SOURCES = testSimpleCamera.cpp testSimpleCamera_LDADD = libgtsam.la # Visual SLAM -headers += VSLAMConfig.h -sources += VSLAMGraph.cpp VSLAMFactor.cpp +sources += visualSLAM.cpp check_PROGRAMS += testVSLAMFactor testVSLAMGraph testVSLAMConfig testVSLAMFactor_SOURCES = testVSLAMFactor.cpp testVSLAMFactor_LDADD = libgtsam.la diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index d4a3d0dd9..2625266e8 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -20,6 +20,11 @@ #include "Matrix.h" #include "GaussianFactor.h" +#define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \ + template class gtsam::NonlinearFactor1; +#define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,X1,J2,X2) \ + template class gtsam::NonlinearFactor2; + namespace gtsam { // TODO class NoiseModel {}; @@ -173,7 +178,7 @@ namespace gtsam { return Base::equals(*p, tol) && (key_ == p->key_); } - /** error function z-h(x) */ + /** error function h(x)-z */ inline Vector error_vector(const Config& x) const { Key j = key_; const X& xj = x[j]; diff --git a/cpp/NonlinearFactorGraph-inl.h b/cpp/NonlinearFactorGraph-inl.h index b0dce3ede..e1d84cbc0 100644 --- a/cpp/NonlinearFactorGraph-inl.h +++ b/cpp/NonlinearFactorGraph-inl.h @@ -11,6 +11,11 @@ #include #include "GaussianFactorGraph.h" #include "NonlinearFactorGraph.h" +#include "FactorGraph-inl.h" + +#define INSTANTIATE_NONLINEAR_FACTOR_GRAPH(C) \ + INSTANTIATE_FACTOR_GRAPH(NonlinearFactor); \ + template class NonlinearFactorGraph; using namespace std; diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h index b57423db2..c031f20ee 100644 --- a/cpp/NonlinearFactorGraph.h +++ b/cpp/NonlinearFactorGraph.h @@ -41,6 +41,11 @@ namespace gtsam { return exp(-0.5 * error(c)); } + template + void add(const F& factor) { + push_back(boost::shared_ptr(new F(factor))); + } + /** * linearize a nonlinear factor graph */ diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index ed2776ad8..c12ae7593 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -13,6 +13,9 @@ #include #include "NonlinearOptimizer.h" +#define INSTANTIATE_NONLINEAR_OPTIMIZER(G,C) \ + template class NonlinearOptimizer; + using namespace std; namespace gtsam { diff --git a/cpp/Point2.cpp b/cpp/Point2.cpp index 9af53dda1..eb0dba421 100644 --- a/cpp/Point2.cpp +++ b/cpp/Point2.cpp @@ -12,7 +12,7 @@ using namespace std; namespace gtsam { /** Explicit instantiation of base class to export members */ - template class Lie; + INSTANTIATE_LIE(Point2); /* ************************************************************************* */ void Point2::print(const string& s) const { diff --git a/cpp/Point3.cpp b/cpp/Point3.cpp index ac7c5b7a3..d7e5c82ed 100644 --- a/cpp/Point3.cpp +++ b/cpp/Point3.cpp @@ -10,7 +10,7 @@ namespace gtsam { /** Explicit instantiation of base class to export members */ - template class Lie; + INSTANTIATE_LIE(Point3); /* ************************************************************************* */ bool Point3::equals(const Point3 & q, double tol) const { diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index bead4aa5f..1cb4c144d 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -11,7 +11,7 @@ using namespace std; namespace gtsam { /** Explicit instantiation of base class to export members */ - template class Lie; + INSTANTIATE_LIE(Pose2); /* ************************************************************************* */ void Pose2::print(const string& s) const { @@ -46,23 +46,61 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix Dbetween1(const Pose2& p1, const Pose2& p2) { - Point2 dt = p2.t()-p1.t(); - Matrix dT1 = -invcompose(p2.r(), p1.r()).matrix(); - Matrix dR1; - unrotate(p2.r(), dt, dR1); - return Matrix_(3,3, - dT1(0,0), dT1(0,1), dR1(0,0), - dT1(1,0), dT1(1,1), dR1(1,0), - 0.0, 0.0, -1.0); - } + Pose2 between(const Pose2& p1, const Pose2& p2, boost::optional H1, + boost::optional H2) { + Rot2 dR = between(p1.r(), p2.r()); + Point2 dt = p2.t() - p1.t(); + Point2 q = unrotate(p1.r(), dt); + Pose2 dp(dR, q); + if (H1) { + Matrix dT1 = -invcompose(p2.r(), p1.r()).matrix(); + Matrix dR1; + unrotate(p2.r(), dt, dR1); // FD to Richard: I do *not* understand this + *H1 = Matrix_(3,3, + dT1(0,0), dT1(0,1), dR1(0,0), + dT1(1,0), dT1(1,1), dR1(1,0), + 0.0, 0.0, -1.0); + } + if (H2) *H2 = Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); + return dp; + } - Matrix Dbetween2(const Pose2& p1, const Pose2& p2) { - return Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); - } + /* ************************************************************************* */ + Rot2 bearing(const Pose2& pose, const Point2& point) { + Point2 d = transform_to(pose, point); + return relativeBearing(d); + } + + Rot2 bearing(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2) { + if (!H1 && !H2) return bearing(pose, point); + Point2 d = transform_to(pose, point); + Matrix D_result_d; + Rot2 result = relativeBearing(d, D_result_d); + if (H1) *H1 = D_result_d * Dtransform_to1(pose, point); + if (H2) *H2 = D_result_d * Dtransform_to2(pose, point); + return result; + } + + /* ************************************************************************* */ + double range(const Pose2& pose, const Point2& point) { + Point2 d = transform_to(pose, point); + return d.norm(); + } + + double range(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2) { + if (!H1 && !H2) return range(pose, point); + Point2 d = transform_to(pose, point); + double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); + Matrix D_result_d = Matrix_(1, 2, x / n, y / n); + if (H1) *H1 = D_result_d * Dtransform_to1(pose, point); + if (H2) *H2 = D_result_d * Dtransform_to2(pose, point); + return n; + } /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 4c5f17ce2..5bb443c47 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -98,11 +98,8 @@ namespace gtsam { return rotate(pose.r(), point)+pose.t(); } /** Return relative pose between p1 and p2, in p1 coordinate frame */ - /** todo: make sure compiler finds this version of between. */ - //inline Pose2 between(const Pose2& p0, const Pose2& p2) { - // return Pose2(p0.r().invcompose(p2.r()), p0.r().unrotate(p2.t()-p0.t())); } - Matrix Dbetween1(const Pose2& p0, const Pose2& p2); - Matrix Dbetween2(const Pose2& p0, const Pose2& p2); + Pose2 between(const Pose2& p1, const Pose2& p2, + boost::optional H1, boost::optional H2); /** same as compose (pre-multiply this*p1) */ inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0); } @@ -112,7 +109,33 @@ namespace gtsam { inline Point2 operator*(const Pose2& pose, const Point2& point) { return transform_from(pose, point); } + /** + * Calculate bearing to a landmark + * @param pose 2D pose of robot + * @param point 2D location of landmark + * @return 2D rotation \in SO(2) + */ + Rot2 bearing(const Pose2& pose, const Point2& point); + /** + * Calculate bearing and optional derivative(s) + */ + Rot2 bearing(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2); + + /** + * Calculate range to a landmark + * @param pose 2D pose of robot + * @param point 2D location of landmark + * @return range (double) + */ + double range(const Pose2& pose, const Point2& point); + + /** + * Calculate range and optional derivative(s) + */ + double range(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2); } // namespace gtsam diff --git a/cpp/Pose2Graph.cpp b/cpp/Pose2Graph.cpp index 92d926963..1ea49856c 100644 --- a/cpp/Pose2Graph.cpp +++ b/cpp/Pose2Graph.cpp @@ -10,23 +10,41 @@ #include "NonlinearOptimizer-inl.h" #include "NonlinearEquality.h" #include "graph-inl.h" +#include "LieConfig-inl.h" using namespace std; using namespace gtsam; namespace gtsam { + /** Explicit instantiation of templated methods and functions */ + template class LieConfig,Pose2>; + template size_t dim(const Pose2Config& c); + template Pose2Config expmap(const Pose2Config& c, const Vector& delta); + template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta); + // explicit instantiation so all the code is there and we can link with it template class FactorGraph > ; template class NonlinearFactorGraph ; template class NonlinearEquality ; template class NonlinearOptimizer; + /* ************************************************************************* */ + Pose2Config pose2Circle(size_t n, double R) { + Pose2Config x; + double theta = 0, dtheta = 2*M_PI/n; + for(size_t i=0;i (key, pose))); } + /* ************************************************************************* */ bool Pose2Graph::equals(const Pose2Graph& p, double tol) const { return false; } diff --git a/cpp/Pose2Graph.h b/cpp/Pose2Graph.h index 31ab5c98d..986e07666 100644 --- a/cpp/Pose2Graph.h +++ b/cpp/Pose2Graph.h @@ -7,11 +7,31 @@ #pragma once -#include "Pose2Factor.h" #include "NonlinearFactorGraph.h" +#include "Pose2.h" +#include "LieConfig.h" +#include "BetweenFactor.h" +#include "Key.h" namespace gtsam { + /** + * Pose2Config is now simply a typedef + */ + typedef LieConfig,Pose2> Pose2Config; + + typedef BetweenFactor Pose2Factor; + + /** + * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) + * @param n number of poses + * @param R radius of circle + * @param c character to use for keys + * @return circle of n 2D poses + */ + Pose2Config pose2Circle(size_t n, double R); + + /** * Non-linear factor graph for visual SLAM */ diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index bc6fb5179..9e1377e74 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -6,6 +6,7 @@ #include #include "Pose3.h" #include "Lie-inl.h" +#include "LieConfig.h" using namespace std; using namespace boost::numeric::ublas; @@ -13,7 +14,7 @@ using namespace boost::numeric::ublas; namespace gtsam { /** Explicit instantiation of base class to export members */ - template class Lie; + INSTANTIATE_LIE(Pose3); /* ************************************************************************* */ void Pose3::print(const string& s) const { @@ -185,16 +186,14 @@ namespace gtsam { /* ************************************************************************* */ // between = compose(p2,inverse(p1)); - - Matrix Dbetween1(const Pose3& p1, const Pose3& p2){ - Pose3 invp1 = inverse(p1); - return Dcompose2(p2,invp1) * Dinverse(p1); - } - - Matrix Dbetween2(const Pose3& p1, const Pose3& p2){ - Pose3 invp1 = inverse(p1); - return Dcompose1(p2,invp1); - } + Pose3 between(const Pose3& p1, const Pose3& p2, boost::optional H1, + boost::optional H2) { + Pose3 invp1 = inverse(p1); + Pose3 result = compose(p2, invp1); + if (H1) *H1 = Dcompose2(p2, invp1) * Dinverse(p1); + if (H2) *H2 = Dcompose1(p2, invp1); + return result; + } /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Pose3.h b/cpp/Pose3.h index af05b5205..7b3a06c6e 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -46,6 +46,9 @@ namespace gtsam { const Rot3& rotation() const { return R_; } const Point3& translation() const { return t_; } + double x() const { return t_.x(); } + double y() const { return t_.y(); } + double z() const { return t_.z(); } /** convert to 4*4 matrix */ Matrix matrix() const; @@ -110,7 +113,6 @@ namespace gtsam { return concatVectors(2, &r, &t); } - /** receives the point in Pose coordinates and transforms it to world coordinates */ Point3 transform_from(const Pose3& pose, const Point3& p); inline Point3 operator*(const Pose3& pose, const Point3& p) { return transform_from(pose, p); } @@ -135,9 +137,10 @@ namespace gtsam { /** * Return relative pose between p1 and p2, in p1 coordinate frame + * as well as optionally the derivatives */ - Matrix Dbetween1(const Pose3& p1, const Pose3& p2); - Matrix Dbetween2(const Pose3& p1, const Pose3& p2); + Pose3 between(const Pose3& p1, const Pose3& p2, + boost::optional H1, boost::optional H2); /** direct measurement of a pose */ Vector hPose(const Vector& x); diff --git a/cpp/Pose3Config.h b/cpp/Pose3Config.h index ac8fbacf3..16757c871 100644 --- a/cpp/Pose3Config.h +++ b/cpp/Pose3Config.h @@ -8,6 +8,7 @@ #include "Pose3.h" #include "LieConfig.h" +#include "Key.h" namespace gtsam { diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp index bd216796c..40c763318 100644 --- a/cpp/Rot2.cpp +++ b/cpp/Rot2.cpp @@ -12,8 +12,8 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - template class Lie ; + /** Explicit instantiation of base class to export members */ + INSTANTIATE_LIE(Rot2); /* ************************************************************************* */ void Rot2::print(const string& s) const { diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index dc202bbf7..166b550b0 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -14,7 +14,7 @@ using namespace std; namespace gtsam { /** Explicit instantiation of base class to export members */ - template class Lie; + INSTANTIATE_LIE(Rot3); /* ************************************************************************* */ // static member functions to construct rotations diff --git a/cpp/TupleConfig-inl.h b/cpp/TupleConfig-inl.h new file mode 100644 index 000000000..8e92a4c71 --- /dev/null +++ b/cpp/TupleConfig-inl.h @@ -0,0 +1,28 @@ +/* + * TupleConfig-inl.h + * + * Created on: Jan 14, 2010 + * Author: richard + */ + +#include "LieConfig-inl.h" + +#include "TupleConfig.h" + +#define INSTANTIATE_PAIR_CONFIG(J1,X1,J2,X2) \ + /*INSTANTIATE_LIE_CONFIG(J1,X1);*/ \ + /*INSTANTIATE_LIE_CONFIG(J2,X2);*/ \ + template class PairConfig; \ + /*template void PairConfig::print(const std::string&) const;*/ \ + template PairConfig expmap(PairConfig, const VectorConfig&); + +namespace gtsam { + + template + void PairConfig::print(const std::string& s) const { + std::cout << "TupleConfig " << s << ", size " << size_ << "\n"; + first.print(s + "Config1: "); + second.print(s + "Config2: "); + } + +} diff --git a/cpp/TupleConfig.h b/cpp/TupleConfig.h index bd88b3c34..d28fd4f23 100644 --- a/cpp/TupleConfig.h +++ b/cpp/TupleConfig.h @@ -5,9 +5,7 @@ * Author: Richard Roberts and Manohar Paluri */ -#include - -#include "LieConfig-inl.h" +#include "LieConfig.h" #pragma once @@ -18,22 +16,24 @@ namespace gtsam { */ template class PairConfig : public Testable > { + public: -// typedef J1 Key1; -// typedef J2 Key2; -// typedef X1 Value1; -// typedef X2 Value2; + + // publicly available types typedef LieConfig Config1; typedef LieConfig Config2; + // Two configs in the pair as in std:pair + LieConfig first; + LieConfig second; + private: - LieConfig config1_; - LieConfig config2_; + size_t size_; size_t dim_; PairConfig(const LieConfig& config1, const LieConfig& config2) : - config1_(config1), config2_(config2), + first(config1), second(config2), size_(config1.size()+config2.size()), dim_(gtsam::dim(config1)+gtsam::dim(config2)) {} public: @@ -47,30 +47,26 @@ namespace gtsam { * Copy constructor */ PairConfig(const PairConfig& c): - config1_(c.config1_), config2_(c.config2_), size_(c.size_), dim_(c.dim_) {} + first(c.first), second(c.second), size_(c.size_), dim_(c.dim_) {} /** * Print */ - void print(const std::string& s = "") const { - std::cout << "TupleConfig " << s << ", size " << size_ << "\n"; - config1_.print(s + "Config1: "); - config2_.print(s + "Config2: "); - } + void print(const std::string& s = "") const; /** * Test for equality in keys and values */ bool equals(const PairConfig& c, double tol=1e-9) const { - return config1_.equals(c.config1_, tol) && config2_.equals(c.config2_, tol); } + return first.equals(c.first, tol) && second.equals(c.second, tol); } /** * operator[] syntax to get a value by j, throws invalid_argument if * value with specified j is not present. Will generate compile-time * errors if j type does not match that on which the Config is templated. */ - const X1& operator[](const J1& j) const { return config1_[j]; } - const X2& operator[](const J2& j) const { return config2_[j]; } + const X1& operator[](const J1& j) const { return first[j]; } + const X2& operator[](const J2& j) const { return second[j]; } /** * size is the total number of variables in this config. @@ -103,26 +99,26 @@ namespace gtsam { * expmap each element */ PairConfig expmap(const VectorConfig& delta) { - return PairConfig(gtsam::expmap(config1_, delta), gtsam::expmap(config2_, delta)); } + return PairConfig(gtsam::expmap(first, delta), gtsam::expmap(second, delta)); } /** * Insert a variable with the given j */ - void insert(const J1& j, const X1& value) { insert_helper(config1_, j, value); } - void insert(const J2& j, const X2& value) { insert_helper(config2_, j, value); } + void insert(const J1& j, const X1& value) { insert_helper(first, j, value); } + void insert(const J2& j, const X2& value) { insert_helper(second, j, value); } /** * Remove the variable with the given j. Throws invalid_argument if the * j is not present in the config. */ - void erase(const J1& j) { erase_helper(config1_, j); } - void erase(const J2& j) { erase_helper(config2_, j); } + void erase(const J1& j) { erase_helper(first, j); } + void erase(const J2& j) { erase_helper(second, j); } /** * Check if a variable exists */ - bool exists(const J1& j) const { return config1_.exists(j); } - bool exists(const J2& j) const { return config2_.exists(j); } + bool exists(const J1& j) const { return first.exists(j); } + bool exists(const J2& j) const { return second.exists(j); } }; diff --git a/cpp/VSLAMConfig.cpp b/cpp/VSLAMConfig.cpp new file mode 100644 index 000000000..54bb3c810 --- /dev/null +++ b/cpp/VSLAMConfig.cpp @@ -0,0 +1,20 @@ +/** + * @file VSLAMConfig.cpp + * @brief LieConfig instantiations + * @author Frank Dellaert + */ + +#include "VSLAMConfig.h" +#include "LieConfig-inl.h" +#include "TupleConfig.h" + +namespace gtsam { + + // template class LieConfig ; // not this one as duplicate + template class LieConfig ; + template class PairConfig ; + +/* ************************************************************************* */ + +} // namespace gtsam + diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp deleted file mode 100644 index f35a66fa4..000000000 --- a/cpp/VSLAMFactor.cpp +++ /dev/null @@ -1,42 +0,0 @@ -/** - * @file VSLAMFactor.cpp - * @brief A non-linear factor specialized to the Visual SLAM problem - * @author Alireza Fathi - */ - -#include -#include - -#include "VSLAMFactor.h" -#include "SimpleCamera.h" - -using namespace std; -namespace gtsam { - - template class NonlinearFactor2; - - /* ************************************************************************* */ - VSLAMFactor::VSLAMFactor() { - /// Arbitrary values - K_ = shared_ptrK(new Cal3_S2(444, 555, 666, 777, 888)); - } - /* ************************************************************************* */ - VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, - const shared_ptrK &K) : - VSLAMFactorBase(sigma, cn, ln), z_(z), K_(K) { - } - - /* ************************************************************************* */ - void VSLAMFactor::print(const std::string& s) const { - VSLAMFactorBase::print(s); - z_.print(s + ".z"); - } - - /* ************************************************************************* */ - bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const { - return VSLAMFactorBase::equals(p, tol) && z_.equals(p.z_, tol) - && K_->equals(*p.K_, tol); - } - -/* ************************************************************************* */ -} // namespace gtsam diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h deleted file mode 100644 index 7dda532d7..000000000 --- a/cpp/VSLAMFactor.h +++ /dev/null @@ -1,91 +0,0 @@ -/** - * @file VSLAMFactor.h - * @brief A Nonlinear Factor, specialized for visual SLAM - * @author Alireza Fathi - */ - -#pragma once - -#include - -#include "NonlinearFactor.h" -#include "SimpleCamera.h" -#include "VSLAMConfig.h" -#include "Cal3_S2.h" - -namespace gtsam { - - typedef NonlinearFactor2 VSLAMFactorBase; - - /** - * Non-linear factor for a constraint derived from a 2D measurement, - * i.e. the main building block for visual SLAM. - */ - class VSLAMFactor: public VSLAMFactorBase , Testable { - private: - - // Keep a copy of measurement and calibration for I/O - Point2 z_; - boost::shared_ptr K_; - - public: - - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /** - * Default constructor - */ - VSLAMFactor(); - - /** - * Constructor - * @param z is the 2 dimensional location of point in image (the measurement) - * @param sigma is the standard deviation - * @param cameraFrameNumber is basically the frame number - * @param landmarkNumber is the index of the landmark - * @param K the constant calibration - */ - VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, - int landmarkNumber, const shared_ptrK & K); - - /** - * print - * @param s optional string naming the factor - */ - void print(const std::string& s = "VSLAMFactor") const; - - /** - * equals - */ - bool equals(const VSLAMFactor&, double tol = 1e-9) const; - - /** h(x) */ - Point2 predict(const Pose3& pose, const Point3& point) const { - return SimpleCamera(*K_, pose).project(point); - } - - /** h(x)-z */ - Vector evaluateError(const Pose3& pose, const Point3& point, - boost::optional H1, boost::optional H2) const { - SimpleCamera camera(*K_, pose); - if (H1) *H1=Dproject_pose(camera,point); - if (H2) *H2=Dproject_point(camera,point); - Point2 reprojectionError(project(camera, point) - z_); - return reprojectionError.vector(); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - //ar & BOOST_SERIALIZATION_NVP(key1_); - //ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(z_); - ar & BOOST_SERIALIZATION_NVP(K_); - } - }; - -} diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp deleted file mode 100644 index bce5d344e..000000000 --- a/cpp/VSLAMGraph.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/** - * @file VSLAMGraph.h - * @brief A factor graph for the VSLAM problem - * @author Alireza Fathi - * @author Carlos Nieto - */ - -#include -#include -#include - -#include "VSLAMGraph.h" -#include "NonlinearFactorGraph-inl.h" -#include "NonlinearOptimizer-inl.h" -#include "NonlinearEquality.h" - -using namespace std; - -namespace gtsam { - -// explicit instantiation so all the code is there and we can link with it -template class FactorGraph >; -template class NonlinearFactorGraph; -template class NonlinearOptimizer; - -/* ************************************************************************* */ -bool compareLandmark(const std::string& key, - const VSLAMConfig& feasible, - const VSLAMConfig& input) { - int j = atoi(key.substr(1, key.size() - 1).c_str()); - return feasible[VSLAMPointKey(j)].equals(input[VSLAMPointKey(j)]); -} - -/* ************************************************************************* */ -void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) { - typedef NonlinearEquality NLE; - boost::shared_ptr factor(new NLE(j, p)); - push_back(factor); -} - -/* ************************************************************************* */ -bool compareCamera(const std::string& key, - const VSLAMConfig& feasible, - const VSLAMConfig& input) { - int j = atoi(key.substr(1, key.size() - 1).c_str()); - return feasible[VSLAMPoseKey(j)].equals(input[VSLAMPoseKey(j)]); -} - -/* ************************************************************************* */ -void VSLAMGraph::addCameraConstraint(int j, const gtsam::Pose3& p) { - typedef NonlinearEquality NLE; - boost::shared_ptr factor(new NLE(j,p)); - push_back(factor); -} - -/* ************************************************************************* */ - -} // namespace gtsam - diff --git a/cpp/VSLAMGraph.h b/cpp/VSLAMGraph.h deleted file mode 100644 index 24ff9557d..000000000 --- a/cpp/VSLAMGraph.h +++ /dev/null @@ -1,68 +0,0 @@ -/** - * @file VSLAMGraph.h - * @brief A factor graph for the VSLAM problem - * @author Alireza Fathi - * @author Carlos Nieto - */ - -#pragma once - -#include -#include -#include -#include - -#include "VSLAMFactor.h" -#include "NonlinearFactorGraph.h" -#include "FactorGraph-inl.h" -#include "VSLAMConfig.h" -#include "Testable.h" - -namespace gtsam{ - -/** - * Non-linear factor graph for visual SLAM - */ -class VSLAMGraph : public gtsam::NonlinearFactorGraph{ - -public: - - /** default constructor is empty graph */ - VSLAMGraph() {} - - /** - * print out graph - */ - void print(const std::string& s = "") const { - gtsam::NonlinearFactorGraph::print(s); - } - - /** - * equals - */ - bool equals(const VSLAMGraph& p, double tol=1e-9) const { - return gtsam::NonlinearFactorGraph::equals(p, tol); - } - - /** - * Add a constraint on a landmark (for now, *must* be satisfied in any Config) - * @param j index of landmark - * @param p to which point to constrain it to - */ - void addLandmarkConstraint(int j, const Point3& p = Point3()); - - /** - * Add a constraint on a camera (for now, *must* be satisfied in any Config) - * @param j index of camera - * @param p to which pose to constrain it to - */ - void addCameraConstraint(int j, const Pose3& p = Pose3()); - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) {} -}; - -} // namespace gtsam diff --git a/cpp/graph-inl.h b/cpp/graph-inl.h index ede7b16a9..8a927c656 100644 --- a/cpp/graph-inl.h +++ b/cpp/graph-inl.h @@ -17,12 +17,6 @@ using namespace std; namespace gtsam { -// some typedefs we need - -//typedef boost::graph_traits::vertex_iterator BoostVertexIterator; - - - /* ************************************************************************* */ template SDGraph toBoostGraph(const G& graph) { @@ -31,13 +25,17 @@ SDGraph toBoostGraph(const G& graph) { typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; map key2vertex; BoostVertex v1, v2; - BOOST_FOREACH(F factor, graph) { - if (factor->keys().size() > 2) + typename G::const_iterator itFactor; + for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) { + if ((*itFactor)->keys().size() > 2) throw(invalid_argument("toBoostGraph: only support factors with at most two keys")); - if (factor->keys().size() == 1) + if ((*itFactor)->keys().size() == 1) continue; + boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); + if (!factor) continue; + Key key1 = factor->key1(); Key key2 = factor->key2(); diff --git a/cpp/planarSLAM.cpp b/cpp/planarSLAM.cpp new file mode 100644 index 000000000..b8cc3c3fa --- /dev/null +++ b/cpp/planarSLAM.cpp @@ -0,0 +1,47 @@ +/** + * @file planarSLAM.cpp + * @brief: bearing/range measurements in 2D plane + * @authors Frank Dellaert + **/ + +#include "planarSLAM.h" +#include "NonlinearFactorGraph-inl.h" +#include "NonlinearOptimizer-inl.h" +#include "TupleConfig-inl.h" + +// Use planarSLAM namespace for specific SLAM instance +namespace gtsam { + + using namespace planarSLAM; + INSTANTIATE_PAIR_CONFIG(PoseKey, Pose2, PointKey, Point2) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) + INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) + + namespace planarSLAM { + + void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { + sharedFactor factor(new Constraint(i, p)); + push_back(factor); + } + + void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, + const Matrix& cov) { + sharedFactor factor(new Odometry(i, j, z, cov)); + push_back(factor); + } + + void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, + double sigma) { + sharedFactor factor(new Bearing(i, j, z, sigma)); + push_back(factor); + } + + void Graph::addRange(const PoseKey& i, const PointKey& j, double z, + double sigma) { + sharedFactor factor(new Range(i, j, z, sigma)); + push_back(factor); + } + + } // planarSLAM + +} // gtsam diff --git a/cpp/planarSLAM.h b/cpp/planarSLAM.h new file mode 100644 index 000000000..9678dee22 --- /dev/null +++ b/cpp/planarSLAM.h @@ -0,0 +1,106 @@ +/** + * @file planarSLAM.h + * @brief: bearing/range measurements in 2D plane + * @authors Frank Dellaert + **/ + +#pragma once + +#include "Key.h" +#include "Pose2.h" +#include "Point2.h" +#include "NonlinearFactor.h" +#include "TupleConfig.h" +#include "NonlinearEquality.h" +#include "BetweenFactor.h" +#include "NonlinearFactorGraph.h" +#include "NonlinearOptimizer.h" + +// We use gtsam namespace for generally useful factors +namespace gtsam { + + /** + * Binary factor for a bearing measurement + */ + template + class BearingFactor: public NonlinearFactor2 { + private: + + Rot2 z_; /** measurement */ + + typedef NonlinearFactor2 Base; + + public: + + BearingFactor(); /* Default constructor */ + BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z, + double sigma) : + Base(sigma, i, j), z_(z) { + } + + /** h(x)-z -> between(z,h(x)) for Rot2 manifold */ + Vector evaluateError(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2) const { + Rot2 hx = bearing(pose, point, H1, H2); + return logmap(between(z_, hx)); + } + }; // BearingFactor + + /** + * Binary factor for a range measurement + */ + template + class RangeFactor: public NonlinearFactor2 { + private: + + double z_; /** measurement */ + + typedef NonlinearFactor2 Base; + + public: + + RangeFactor(); /* Default constructor */ + + RangeFactor(const PoseKey& i, const PointKey& j, double z, double sigma) : + Base(sigma, i, j), z_(z) { + } + + /** h(x)-z */ + Vector evaluateError(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2) const { + double hx = gtsam::range(pose, point, H1, H2); + return Vector_(1, hx - z_); + } + }; // RangeFactor + + // Use planarSLAM namespace for specific SLAM instance + namespace planarSLAM { + + // Keys and Config + typedef Symbol PoseKey; + typedef Symbol PointKey; + typedef PairConfig Config; + + // Factors + typedef NonlinearEquality Constraint; + typedef BetweenFactor Odometry; + typedef BearingFactor Bearing; + typedef RangeFactor Range; + + // Graph + struct Graph: public NonlinearFactorGraph { + void addPoseConstraint(const PoseKey& i, const Pose2& p); + void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, const Matrix& cov); + void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, double sigma); + void addRange(const PoseKey& i, const PointKey& j, double z, double sigma); + }; + + // Optimizer + typedef NonlinearOptimizer Optimizer; + + } // planarSLAM + +} // namespace gtsam + diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 1acfb8216..b579a4591 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -743,7 +743,7 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree ) g.add("x2", I, "x4", I, b, 0); g.add("x3", I, "x4", I, b, 0); - map tree = g.findMinimumSpanningTree(); + map tree = g.findMinimumSpanningTree(); CHECK(tree["x1"].compare("x1")==0); CHECK(tree["x2"].compare("x1")==0); CHECK(tree["x3"].compare("x1")==0); @@ -762,14 +762,14 @@ TEST( GaussianFactorGraph, split ) g.add("x2", I, "x3", I, b, 0); g.add("x2", I, "x4", I, b, 0); - map tree; + PredecessorMap tree; tree["x1"] = "x1"; tree["x2"] = "x1"; tree["x3"] = "x1"; tree["x4"] = "x1"; GaussianFactorGraph Ab1, Ab2; - g.split(tree, Ab1, Ab2); + g.split(tree, Ab1, Ab2); LONGS_EQUAL(3, Ab1.size()); LONGS_EQUAL(2, Ab2.size()); } diff --git a/cpp/testOrdering.cpp b/cpp/testOrdering.cpp index 23beed59f..eb1aa735a 100644 --- a/cpp/testOrdering.cpp +++ b/cpp/testOrdering.cpp @@ -6,7 +6,7 @@ #include // for operator += #include #include "Ordering-inl.h" -#include "Pose2Config.h" +#include "Pose2Graph.h" using namespace std; diff --git a/cpp/testPlanarSLAM.cpp b/cpp/testPlanarSLAM.cpp new file mode 100644 index 000000000..2885c8ab2 --- /dev/null +++ b/cpp/testPlanarSLAM.cpp @@ -0,0 +1,90 @@ +/** + * @file testPlanarSLAM.cpp + * @authors Frank Dellaert + **/ + +#include +#include +#include "planarSLAM.h" + +using namespace std; +using namespace gtsam; + +// some shared test values +Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); +Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); + +/* ************************************************************************* */ +TEST( planarSLAM, BearingFactor ) +{ + // Create factor + Rot2 z(M_PI_4 + 0.1); // h(x) - z = -0.1 + double sigma = 0.1; + planarSLAM::Bearing factor(2, 3, z, sigma); + + // create config + planarSLAM::Config c; + c.insert(2, x2); + c.insert(3, l3); + + // Check error + Vector actual = factor.error_vector(c); + CHECK(assert_equal(Vector_(1,-0.1),actual)); +} + +/* ************************************************************************* */ +TEST( planarSLAM, RangeFactor ) +{ + // Create factor + double z(sqrt(2) - 0.22); // h(x) - z = 0.22 + double sigma = 0.1; + planarSLAM::Range factor(2, 3, z, sigma); + + // create config + planarSLAM::Config c; + c.insert(2, x2); + c.insert(3, l3); + + // Check error + Vector actual = factor.error_vector(c); + CHECK(assert_equal(Vector_(1,0.22),actual)); +} + +/* ************************************************************************* */ +TEST( planarSLAM, constructor ) +{ + // create config + planarSLAM::Config c; + c.insert(2, x2); + c.insert(3, x3); + c.insert(3, l3); + + // create graph + planarSLAM::Graph G; + + // Add pose constraint + G.addPoseConstraint(2, x2); // make it feasible :-) + + // Add odometry + G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), eye(3)); + + // Create bearing factor + Rot2 z1(M_PI_4 + 0.1); // h(x) - z = -0.1 + double sigma1 = 0.1; + G.addBearing(2, 3, z1, sigma1); + + // Create range factor + double z2(sqrt(2) - 0.22); // h(x) - z = 0.22 + double sigma2 = 0.1; + G.addRange(2, 3, z2, sigma2); + + Vector expected = Vector_(8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.22); + CHECK(assert_equal(expected,G.error_vector(c))); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index e58f24db3..38839eabf 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -152,13 +152,20 @@ TEST(Pose2, compose_c) /* ************************************************************************* */ TEST( Pose2, between ) { - //cout << "between" << endl; + // < + // + // ^ + // + // *--0--*--* Pose2 p1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 p2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x + Matrix actualH1,actualH2; Pose2 expected(M_PI_2, Point2(2,2)); - Pose2 actual = between(p1,p2); - CHECK(assert_equal(expected,actual)); + Pose2 actual1 = between(p1,p2); + Pose2 actual2 = between(p1,p2,actualH1,actualH2); + CHECK(assert_equal(expected,actual1)); + CHECK(assert_equal(expected,actual2)); Matrix expectedH1 = Matrix_(3,3, 0.0,-1.0,-2.0, @@ -166,7 +173,6 @@ TEST( Pose2, between ) 0.0, 0.0,-1.0 ); Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5); - Matrix actualH1 = Dbetween1(p1,p2); CHECK(assert_equal(expectedH1,actualH1)); CHECK(assert_equal(numericalH1,actualH1)); @@ -176,11 +182,25 @@ TEST( Pose2, between ) 0.0, 0.0, 1.0 ); Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5); - Matrix actualH2 = Dbetween2(p1,p2); CHECK(assert_equal(expectedH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2)); } +/* ************************************************************************* */ +// reverse situation for extra test +TEST( Pose2, between2 ) +{ + Pose2 p2(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y + Pose2 p1(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x + + Matrix actualH1,actualH2; + between(p1,p2,actualH1,actualH2); + Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5); + CHECK(assert_equal(numericalH1,actualH1)); + Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5); + CHECK(assert_equal(numericalH2,actualH2)); +} + /* ************************************************************************* */ TEST( Pose2, round_trip ) { @@ -197,6 +217,75 @@ TEST(Pose2, members) CHECK(pose.dim() == 3); } +/* ************************************************************************* */ +// some shared test values +Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); +Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); + +/* ************************************************************************* */ +TEST( Pose2, bearing ) +{ + Matrix expectedH1, actualH1, expectedH2, actualH2; + + // establish bearing is indeed zero + CHECK(assert_equal(Rot2(),bearing(x1,l1))); + + // establish bearing is indeed 45 degrees + CHECK(assert_equal(Rot2(M_PI_4),bearing(x1,l2))); + + // establish bearing is indeed 45 degrees even if shifted + Rot2 actual23 = bearing(x2, l3, actualH1, actualH2); + CHECK(assert_equal(Rot2(M_PI_4),actual23)); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(bearing, x2, l3, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + expectedH2 = numericalDerivative22(bearing, x2, l3, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + + // establish bearing is indeed 45 degrees even if rotated + Rot2 actual34 = bearing(x3, l4, actualH1, actualH2); + CHECK(assert_equal(Rot2(M_PI_4),actual34)); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(bearing, x3, l4, 1e-5); + expectedH2 = numericalDerivative22(bearing, x3, l4, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + CHECK(assert_equal(expectedH1,actualH1)); +} + +/* ************************************************************************* */ +TEST( Pose2, range ) +{ + Matrix expectedH1, actualH1, expectedH2, actualH2; + + // establish range is indeed zero + DOUBLES_EQUAL(1,gtsam::range(x1,l1),1e-9); + + // establish range is indeed 45 degrees + DOUBLES_EQUAL(sqrt(2),gtsam::range(x1,l2),1e-9); + + // Another pair + double actual23 = gtsam::range(x2, l3, actualH1, actualH2); + DOUBLES_EQUAL(sqrt(2),actual23,1e-9); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(range, x2, l3, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + expectedH2 = numericalDerivative22(range, x2, l3, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + + // Another test + double actual34 = gtsam::range(x3, l4, actualH1, actualH2); + DOUBLES_EQUAL(2,actual34,1e-9); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(range, x3, l4, 1e-5); + expectedH2 = numericalDerivative22(range, x3, l4, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + CHECK(assert_equal(expectedH1,actualH1)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testPose2Config.cpp b/cpp/testPose2Config.cpp index 085e20bcf..0a4694e24 100644 --- a/cpp/testPose2Config.cpp +++ b/cpp/testPose2Config.cpp @@ -6,7 +6,7 @@ #include #include -#include "Pose2Config.h" +#include "Pose2Graph.h" using namespace std; using namespace gtsam; diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index 79a2dac79..9c07c967a 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -6,7 +6,7 @@ #include #include "numericalDerivative.h" -#include "Pose2Factor.h" +#include "Pose2Graph.h" using namespace std; using namespace gtsam; diff --git a/cpp/testPose2Graph.cpp b/cpp/testPose2Graph.cpp index 67f16ba0c..6d32f172c 100644 --- a/cpp/testPose2Graph.cpp +++ b/cpp/testPose2Graph.cpp @@ -12,8 +12,8 @@ using namespace boost::assign; #include #include "NonlinearOptimizer-inl.h" +#include "FactorGraph-inl.h" #include "Ordering.h" -#include "Pose2Config.h" #include "Pose2Graph.h" using namespace std; @@ -156,6 +156,44 @@ TEST(Pose2Graph, optimizeCircle) { CHECK(assert_equal(delta,between(actual[5],actual[0]))); } +/* ************************************************************************* */ +// test optimization with 6 poses arranged in a hexagon and a loop closure +TEST(Pose2Graph, findMinimumSpanningTree) { + typedef Pose2Config::Key Key; + + Pose2Graph G, T, C; + Matrix cov = eye(3); + G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(0.,0.,0.), cov))); + G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov))); + G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov))); + + PredecessorMap tree = G.findMinimumSpanningTree(); + CHECK(tree[Key(1)] == Key(1)); + CHECK(tree[Key(2)] == Key(1)); + CHECK(tree[Key(3)] == Key(1)); +} + +/* ************************************************************************* */ +// test optimization with 6 poses arranged in a hexagon and a loop closure +TEST(Pose2Graph, split) { + typedef Pose2Config::Key Key; + + Pose2Graph G, T, C; + Matrix cov = eye(3); + G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(0.,0.,0.), cov))); + G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov))); + G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov))); + + PredecessorMap tree; + tree.insert(Key(1),Key(2)); + tree.insert(Key(2),Key(2)); + tree.insert(Key(3),Key(2)); + + G.split(tree, T, C); + LONGS_EQUAL(2, T.size()); + LONGS_EQUAL(1, C.size()); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp index f9c8d95ca..4924fa5e4 100644 --- a/cpp/testPose3.cpp +++ b/cpp/testPose3.cpp @@ -312,14 +312,13 @@ TEST( Pose3, between ) Pose3 T(R,t); Pose3 expected = pose1 * inverse(T); - Pose3 actual = between(T, pose1); + Matrix actualH1,actualH2; + Pose3 actual = between(T, pose1, actualH1,actualH2); CHECK(assert_equal(expected,actual)); Matrix numericalH1 = numericalDerivative21(between , T, pose1, 1e-5); - Matrix actualH1 = Dbetween1(T, pose1); CHECK(assert_equal(numericalH1,actualH1)); // chain rule does not work ?? - Matrix actualH2 = Dbetween2(T, pose1); Matrix numericalH2 = numericalDerivative22(between , T, pose1, 1e-5); CHECK(assert_equal(numericalH2,actualH2)); } diff --git a/cpp/testRot2.cpp b/cpp/testRot2.cpp index 289bee544..29fe15589 100644 --- a/cpp/testRot2.cpp +++ b/cpp/testRot2.cpp @@ -97,6 +97,29 @@ TEST( Rot2, unrotate) CHECK(assert_equal(numerical2,H2)); } +/* ************************************************************************* */ +TEST( Rot2, relativeBearing ) +{ + Point2 l1(1, 0), l2(1, 1); + Matrix expectedH, actualH; + + // establish relativeBearing is indeed zero + Rot2 actual1 = relativeBearing(l1, actualH); + CHECK(assert_equal(Rot2(),actual1)); + + // Check numerical derivative + expectedH = numericalDerivative11(relativeBearing, l1, 1e-5); + CHECK(assert_equal(expectedH,actualH)); + + // establish relativeBearing is indeed 45 degrees + Rot2 actual2 = relativeBearing(l2, actualH); + CHECK(assert_equal(Rot2(M_PI_4),actual2)); + + // Check numerical derivative + expectedH = numericalDerivative11(relativeBearing, l2, 1e-5); + CHECK(assert_equal(expectedH,actualH)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 65ec0417f..600829cb1 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -12,17 +12,11 @@ #include #include #include -#include -#include -#include #include #include #include #include -#include -#include -#include -#include +#include // templated implementations #include @@ -468,10 +462,13 @@ size_t w=640,h=480; Cal3_S2 K(fov,w,h); boost::shared_ptr shK(new Cal3_S2(K)); +using namespace gtsam::visualSLAM; +using namespace boost; + // typedefs for visual SLAM example -typedef boost::shared_ptr shared_vf; -typedef NonlinearOptimizer VOptimizer; -typedef SQPOptimizer SOptimizer; +typedef boost::shared_ptr shared_vf; +typedef NonlinearOptimizer VOptimizer; +typedef SQPOptimizer SOptimizer; /** * Ground truth for a visual SLAM example with stereo vision @@ -492,26 +489,26 @@ TEST (SQP, stereo_truth ) { Point3 landmarkNoisy(1.0, 6.0, 0.0); // create truth config - boost::shared_ptr truthConfig(new VSLAMConfig); + boost::shared_ptr truthConfig(new Config); truthConfig->insert(1, camera1.pose()); truthConfig->insert(2, camera2.pose()); truthConfig->insert(1, landmark); // create graph - shared_ptr graph(new VSLAMGraph()); + shared_ptr graph(new Graph()); // create equality constraints for poses - graph->addCameraConstraint(1, camera1.pose()); - graph->addCameraConstraint(2, camera2.pose()); + graph->push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); + graph->push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); // create VSLAM factors Point2 z1 = camera1.project(landmark); if (verbose) z1.print("z1"); - shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK)); + shared_vf vf1(new ProjectionFactor(z1, 1.0, 1, 1, shK)); graph->push_back(vf1); Point2 z2 = camera2.project(landmark); if (verbose) z2.print("z2"); - shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK)); + shared_vf vf2(new ProjectionFactor(z2, 1.0, 2, 1, shK)); graph->push_back(vf2); if (verbose) graph->print("Graph after construction"); @@ -559,32 +556,32 @@ TEST (SQP, stereo_truth_noisy ) { Point3 landmarkNoisy(1.0, noisyDist, 0.0); // initial point is too far out // create truth config - boost::shared_ptr truthConfig(new VSLAMConfig); + boost::shared_ptr truthConfig(new Config); truthConfig->insert(1, camera1.pose()); truthConfig->insert(2, camera2.pose()); truthConfig->insert(1, landmark); // create config - boost::shared_ptr noisyConfig(new VSLAMConfig); + boost::shared_ptr noisyConfig(new Config); noisyConfig->insert(1, camera1.pose()); noisyConfig->insert(2, camera2.pose()); noisyConfig->insert(1, landmarkNoisy); // create graph - shared_ptr graph(new VSLAMGraph()); + shared_ptr graph(new Graph()); // create equality constraints for poses - graph->addCameraConstraint(1, camera1.pose()); - graph->addCameraConstraint(2, camera2.pose()); + graph->push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); + graph->push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); // create VSLAM factors Point2 z1 = camera1.project(landmark); if (verbose) z1.print("z1"); - shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK)); + shared_vf vf1(new ProjectionFactor(z1, 1.0, 1, 1, shK)); graph->push_back(vf1); Point2 z2 = camera2.project(landmark); if (verbose) z2.print("z2"); - shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK)); + shared_vf vf2(new ProjectionFactor(z2, 1.0, 2, 1, shK)); graph->push_back(vf2); if (verbose) { @@ -628,25 +625,25 @@ namespace sqp_stereo { // binary constraint between landmarks /** g(x) = x-y = 0 */ - Vector g(const VSLAMConfig& config, const list& keys) { - return config[VSLAMPointKey(getNum(keys.front()))].vector() - - config[VSLAMPointKey(getNum(keys.back()))].vector(); + Vector g(const Config& config, const list& keys) { + return config[PointKey(getNum(keys.front()))].vector() + - config[PointKey(getNum(keys.back()))].vector(); } /** jacobian at l1 */ - Matrix G1(const VSLAMConfig& config, const list& keys) { + Matrix G1(const Config& config, const list& keys) { return eye(3); } /** jacobian at l2 */ - Matrix G2(const VSLAMConfig& config, const list& keys) { + Matrix G2(const Config& config, const list& keys) { return -1.0 * eye(3); } } // \namespace sqp_stereo /* ********************************************************************* */ -VSLAMGraph stereoExampleGraph() { +Graph stereoExampleGraph() { // create initial estimates Rot3 faceDownY(Matrix_(3,3, 1.0, 0.0, 0.0, @@ -660,26 +657,26 @@ VSLAMGraph stereoExampleGraph() { Point3 landmark2(1.0, 5.0, 0.0); // create graph - VSLAMGraph graph; + Graph graph; // create equality constraints for poses - graph.addCameraConstraint(1, camera1.pose()); - graph.addCameraConstraint(2, camera2.pose()); + graph.push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); + graph.push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); - // create VSLAM factors + // create factors Point2 z1 = camera1.project(landmark1); - shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK)); + shared_vf vf1(new ProjectionFactor(z1, 1.0, 1, 1, shK)); graph.push_back(vf1); Point2 z2 = camera2.project(landmark2); - shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 2, shK)); + shared_vf vf2(new ProjectionFactor(z2, 1.0, 2, 2, shK)); graph.push_back(vf2); // create the binary equality constraint between the landmarks // NOTE: this is really just a linear constraint that is exactly the same // as the previous examples list keys; keys += "l1", "l2"; - boost::shared_ptr > c2( - new NonlinearConstraint2( + boost::shared_ptr > c2( + new NonlinearConstraint2( boost::bind(sqp_stereo::g, _1, keys), "l1", boost::bind(sqp_stereo::G1, _1, keys), "l2", boost::bind(sqp_stereo::G2, _1, keys), @@ -690,7 +687,7 @@ VSLAMGraph stereoExampleGraph() { } /* ********************************************************************* */ -boost::shared_ptr stereoExampleTruthConfig() { +boost::shared_ptr stereoExampleTruthConfig() { // create initial estimates Rot3 faceDownY(Matrix_(3,3, 1.0, 0.0, 0.0, @@ -704,7 +701,7 @@ boost::shared_ptr stereoExampleTruthConfig() { Point3 landmark2(1.0, 5.0, 0.0); // create config - boost::shared_ptr truthConfig(new VSLAMConfig); + boost::shared_ptr truthConfig(new Config); truthConfig->insert(1, camera1.pose()); truthConfig->insert(2, camera2.pose()); truthConfig->insert(1, landmark1); @@ -721,11 +718,11 @@ TEST (SQP, stereo_sqp ) { bool verbose = false; // get a graph - VSLAMGraph graph = stereoExampleGraph(); + Graph graph = stereoExampleGraph(); if (verbose) graph.print("Graph after construction"); // get the truth config - boost::shared_ptr truthConfig = stereoExampleTruthConfig(); + boost::shared_ptr truthConfig = stereoExampleTruthConfig(); // create ordering Ordering ord; @@ -749,7 +746,7 @@ TEST (SQP, stereo_sqp_noisy ) { bool verbose = false; // get a graph - VSLAMGraph graph = stereoExampleGraph(); + Graph graph = stereoExampleGraph(); // create initial data Rot3 faceDownY(Matrix_(3,3, @@ -762,7 +759,7 @@ TEST (SQP, stereo_sqp_noisy ) { Point3 landmark2(1.5, 5.0, 0.0); // noisy config - boost::shared_ptr initConfig(new VSLAMConfig); + boost::shared_ptr initConfig(new Config); initConfig->insert(1, pose1); initConfig->insert(2, pose2); initConfig->insert(1, landmark1); @@ -793,7 +790,7 @@ TEST (SQP, stereo_sqp_noisy ) { << "Final Error: " << optimizer.error() << endl; // get the truth config - boost::shared_ptr truthConfig = stereoExampleTruthConfig(); + boost::shared_ptr truthConfig = stereoExampleTruthConfig(); if (verbose) { initConfig->print("Initial Config"); @@ -814,7 +811,7 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) { bool verbose = false; // get a graph - VSLAMGraph graph = stereoExampleGraph(); + Graph graph = stereoExampleGraph(); // create initial data Rot3 faceDownY(Matrix_(3,3, @@ -827,7 +824,7 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) { Point3 landmark2(1.5, 5.0, 0.0); // noisy config - boost::shared_ptr initConfig(new VSLAMConfig); + boost::shared_ptr initConfig(new Config); initConfig->insert(1, pose1); initConfig->insert(2, pose2); initConfig->insert(1, landmark1); @@ -863,7 +860,7 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) { << "Final Error: " << optimizer.error() << endl; // get the truth config - boost::shared_ptr truthConfig = stereoExampleTruthConfig(); + boost::shared_ptr truthConfig = stereoExampleTruthConfig(); if (verbose) { initConfig->print("Initial Config"); diff --git a/cpp/testTupleConfig.cpp b/cpp/testTupleConfig.cpp index 26cf94653..12730cd07 100644 --- a/cpp/testTupleConfig.cpp +++ b/cpp/testTupleConfig.cpp @@ -10,8 +10,10 @@ #include #include -#include "TupleConfig.h" #include "Vector.h" +#include "Key.h" +#include "VectorConfig.h" +#include "TupleConfig-inl.h" using namespace gtsam; using namespace std; diff --git a/cpp/testVSLAMConfig.cpp b/cpp/testVSLAMConfig.cpp index 8ee7d951b..1f9cbe38b 100644 --- a/cpp/testVSLAMConfig.cpp +++ b/cpp/testVSLAMConfig.cpp @@ -1,25 +1,26 @@ /* - * @file testVSLAMConfig.cpp + * @file testConfig.cpp * @brief Tests for the Visual SLAM configuration class * @author Alex Cunningham */ #include #include "VectorConfig.h" -#include "VSLAMConfig.h" +#include "visualSLAM.h" using namespace std; using namespace gtsam; +using namespace gtsam::visualSLAM; /* ************************************************************************* */ -TEST( VSLAMConfig, update_with_large_delta) { +TEST( Config, update_with_large_delta) { // this test ensures that if the update for delta is larger than // the size of the config, it only updates existing variables - VSLAMConfig init; + Config init; init.insert(1, Pose3()); init.insert(1, Point3(1.0, 2.0, 3.0)); - VSLAMConfig expected; + Config expected; expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); expected.insert(1, Point3(1.1, 2.1, 3.1)); @@ -27,7 +28,7 @@ TEST( VSLAMConfig, update_with_large_delta) { delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1)); delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1)); delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1)); - VSLAMConfig actual = expmap(init, delta); + Config actual = expmap(init, delta); CHECK(assert_equal(expected,actual)); } diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index aefaa1f1b..863f621ee 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -4,14 +4,13 @@ #include -#include "VSLAMConfig.h" -#include "VSLAMFactor.h" -#include "VSLAMGraph.h" +#include "visualSLAM.h" #include "Point3.h" #include "Pose3.h" using namespace std; using namespace gtsam; +using namespace gtsam::visualSLAM; // make cube Point3 x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1), @@ -25,20 +24,21 @@ Cal3_S2 K(fov,w,h); // make cameras /* ************************************************************************* */ -TEST( VSLAMFactor, error ) +TEST( ProjectionFactor, error ) { // Create the factor with a measurement that is 3 pixels off in x Point2 z(323.,240.); double sigma=1.0; int cameraFrameNumber=1, landmarkNumber=1; - boost::shared_ptr - factor(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); + boost::shared_ptr + factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); // For the following configuration, the factor predicts 320,240 - VSLAMConfig config; + Config config; Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1); Point3 l1; config.insert(1, l1); - CHECK(assert_equal(Point2(320.,240.),factor->predict(x1,l1))); + // Point should project to Point2(320.,240.) + CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->error_vector(config))); // Which yields an error of 3^2/2 = 4.5 DOUBLES_EQUAL(4.5,factor->error(config),1e-9); @@ -52,7 +52,7 @@ TEST( VSLAMFactor, error ) CHECK(assert_equal(expected,*actual,1e-3)); // linearize graph - VSLAMGraph graph; + Graph graph; graph.push_back(factor); GaussianFactorGraph expected_lfg; expected_lfg.push_back(actual); @@ -63,24 +63,24 @@ TEST( VSLAMFactor, error ) VectorConfig delta; delta.insert("x1",Vector_(6, 0.,0.,0., 1.,1.,1.)); delta.insert("l1",Vector_(3, 1.,2.,3.)); - VSLAMConfig actual_config = expmap(config, delta); - VSLAMConfig expected_config; + Config actual_config = expmap(config, delta); + Config expected_config; Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2); Point3 l2(1,2,3); expected_config.insert(1, l2); CHECK(assert_equal(expected_config,actual_config,1e-9)); } -TEST( VSLAMFactor, equals ) +TEST( ProjectionFactor, equals ) { // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); double sigma=1.0; int cameraFrameNumber=1, landmarkNumber=1; - boost::shared_ptr - factor1(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); + boost::shared_ptr + factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); - boost::shared_ptr - factor2(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); + boost::shared_ptr + factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); CHECK(assert_equal(*factor1, *factor2)); } diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index 52b0b93ed..ef548feea 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -1,5 +1,5 @@ /** - * @file testVSLAMGraph.cpp + * @file testGraph.cpp * @brief Unit test for two cameras and four landmarks * single camera * @author Chris Beall @@ -11,13 +11,16 @@ #include using namespace boost; -#include "VSLAMGraph.h" #include "NonlinearFactorGraph-inl.h" #include "NonlinearOptimizer-inl.h" +#include "Ordering-inl.h" +#include "visualSLAM.h" using namespace std; using namespace gtsam; -typedef NonlinearOptimizer Optimizer; +using namespace gtsam::visualSLAM; +using namespace boost; +typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ Point3 landmark1(-1.0,-1.0, 0.0); @@ -40,7 +43,7 @@ Pose3 camera2(Matrix_(3,3, Point3(0,0,5.00)); /* ************************************************************************* */ -VSLAMGraph testGraph() { +Graph testGraph() { Point2 z11(-100, 100); Point2 z12(-100,-100); Point2 z13( 100,-100); @@ -52,30 +55,30 @@ VSLAMGraph testGraph() { double sigma = 1; shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); - VSLAMGraph g; - g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z11, sigma, 1, 1, sK))); - g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z12, sigma, 1, 2, sK))); - g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z13, sigma, 1, 3, sK))); - g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z14, sigma, 1, 4, sK))); - g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z21, sigma, 2, 1, sK))); - g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z22, sigma, 2, 2, sK))); - g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z23, sigma, 2, 3, sK))); - g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z24, sigma, 2, 4, sK))); + Graph g; + g.add(ProjectionFactor(z11, sigma, 1, 1, sK)); + g.add(ProjectionFactor(z12, sigma, 1, 2, sK)); + g.add(ProjectionFactor(z13, sigma, 1, 3, sK)); + g.add(ProjectionFactor(z14, sigma, 1, 4, sK)); + g.add(ProjectionFactor(z21, sigma, 2, 1, sK)); + g.add(ProjectionFactor(z22, sigma, 2, 2, sK)); + g.add(ProjectionFactor(z23, sigma, 2, 3, sK)); + g.add(ProjectionFactor(z24, sigma, 2, 4, sK)); return g; } /* ************************************************************************* */ -TEST( VSLAMGraph, optimizeLM) +TEST( Graph, optimizeLM) { // build a graph - shared_ptr graph(new VSLAMGraph(testGraph())); + shared_ptr graph(new Graph(testGraph())); // add 3 landmark constraints - graph->addLandmarkConstraint(1, landmark1); - graph->addLandmarkConstraint(2, landmark2); - graph->addLandmarkConstraint(3, landmark3); + graph->add(PointConstraint(1, landmark1)); + graph->add(PointConstraint(2, landmark2)); + graph->add(PointConstraint(3, landmark3)); // Create an initial configuration corresponding to the ground truth - boost::shared_ptr initialEstimate(new VSLAMConfig); + boost::shared_ptr initialEstimate(new Config); initialEstimate->insert(1, camera1); initialEstimate->insert(2, camera2); initialEstimate->insert(1, landmark1); @@ -109,16 +112,16 @@ TEST( VSLAMGraph, optimizeLM) /* ************************************************************************* */ -TEST( VSLAMGraph, optimizeLM2) +TEST( Graph, optimizeLM2) { // build a graph - shared_ptr graph(new VSLAMGraph(testGraph())); + shared_ptr graph(new Graph(testGraph())); // add 2 camera constraints - graph->addCameraConstraint(1, camera1); - graph->addCameraConstraint(2, camera2); + graph->add(PoseConstraint(1, camera1)); + graph->add(PoseConstraint(2, camera2)); // Create an initial configuration corresponding to the ground truth - boost::shared_ptr initialEstimate(new VSLAMConfig); + boost::shared_ptr initialEstimate(new Config); initialEstimate->insert(1, camera1); initialEstimate->insert(2, camera2); initialEstimate->insert(1, landmark1); @@ -153,16 +156,16 @@ TEST( VSLAMGraph, optimizeLM2) /* ************************************************************************* */ -TEST( VSLAMGraph, CHECK_ORDERING) +TEST( Graph, CHECK_ORDERING) { // build a graph - shared_ptr graph(new VSLAMGraph(testGraph())); + shared_ptr graph(new Graph(testGraph())); // add 2 camera constraints - graph->addCameraConstraint(1, camera1); - graph->addCameraConstraint(2, camera2); + graph->add(PoseConstraint(1, camera1)); + graph->add(PoseConstraint(2, camera2)); // Create an initial configuration corresponding to the ground truth - boost::shared_ptr initialEstimate(new VSLAMConfig); + boost::shared_ptr initialEstimate(new Config); initialEstimate->insert(1, camera1); initialEstimate->insert(2, camera2); initialEstimate->insert(1, landmark1); diff --git a/cpp/visualSLAM.cpp b/cpp/visualSLAM.cpp new file mode 100644 index 000000000..093919060 --- /dev/null +++ b/cpp/visualSLAM.cpp @@ -0,0 +1,63 @@ +/* + * visualSLAM.cpp + * + * Created on: Jan 14, 2010 + * Author: richard + */ + +#include "visualSLAM.h" +#include "TupleConfig-inl.h" +#include "NonlinearOptimizer-inl.h" +#include "NonlinearFactorGraph-inl.h" + +namespace gtsam { + + INSTANTIATE_PAIR_CONFIG(visualSLAM::PoseKey, Pose3, visualSLAM::PointKey, Point3) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Config) + INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Config) + + namespace visualSLAM { + + /* ************************************************************************* */ + void ProjectionFactor::print(const std::string& s) const { + Base::print(s); + z_.print(s + ".z"); + } + + /* ************************************************************************* */ + bool ProjectionFactor::equals(const ProjectionFactor& p, double tol) const { + return Base::equals(p, tol) && z_.equals(p.z_, tol) + && K_->equals(*p.K_, tol); + } + + // /* ************************************************************************* */ + // bool compareLandmark(const std::string& key, + // const VSLAMConfig& feasible, + // const VSLAMConfig& input) { + // int j = atoi(key.substr(1, key.size() - 1).c_str()); + // return feasible[VSLAMPointKey(j)].equals(input[VSLAMPointKey(j)]); + // } + // + // /* ************************************************************************* */ + // void VSLAMGraph::addLandmarkConstraint(int j, const Point3& p) { + // typedef NonlinearEquality NLE; + // boost::shared_ptr factor(new NLE(j, p)); + // push_back(factor); + // } + // + // /* ************************************************************************* */ + // bool compareCamera(const std::string& key, + // const VSLAMConfig& feasible, + // const VSLAMConfig& input) { + // int j = atoi(key.substr(1, key.size() - 1).c_str()); + // return feasible[VSLAMPoseKey(j)].equals(input[VSLAMPoseKey(j)]); + // } + // + // /* ************************************************************************* */ + // void VSLAMGraph::addCameraConstraint(int j, const Pose3& p) { + // typedef NonlinearEquality NLE; + // boost::shared_ptr factor(new NLE(j,p)); + // push_back(factor); + // } + } +} diff --git a/cpp/visualSLAM.h b/cpp/visualSLAM.h new file mode 100644 index 000000000..362b07d04 --- /dev/null +++ b/cpp/visualSLAM.h @@ -0,0 +1,152 @@ +/* + * visualSLAM.h + * + * Created on: Jan 14, 2010 + * Author: Richard Roberts and Chris Beall + */ + +#pragma once + +#include "Key.h" +#include "Pose3.h" +#include "Point3.h" +#include "NonlinearFactorGraph.h" +#include "Cal3_S2.h" +#include "Point2.h" +#include "SimpleCamera.h" +#include "TupleConfig.h" +#include "NonlinearEquality.h" + +namespace gtsam { namespace visualSLAM { + + /** + * Typedefs that make up the visualSLAM namespace. + */ + typedef Symbol PoseKey; + typedef Symbol PointKey; + typedef PairConfig Config; + typedef NonlinearFactorGraph Graph; + typedef NonlinearEquality PoseConstraint; + typedef NonlinearEquality PointConstraint; + + + /** + * Non-linear factor for a constraint derived from a 2D measurement, + * i.e. the main building block for visual SLAM. + */ + class ProjectionFactor: public NonlinearFactor2, Testable { + private: + + // Keep a copy of measurement and calibration for I/O + Point2 z_; + boost::shared_ptr K_; + + public: + + // shorthand for base class type + typedef NonlinearFactor2 Base; + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Default constructor + */ + ProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + + /** + * Constructor + * @param z is the 2 dimensional location of point in image (the measurement) + * @param sigma is the standard deviation + * @param cameraFrameNumber is basically the frame number + * @param landmarkNumber is the index of the landmark + * @param K the constant calibration + */ + ProjectionFactor(const Point2& z, double sigma, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K) : + z_(z), K_(K), Base(sigma, j_pose, j_landmark) {} + + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s = "ProjectionFactor") const; + + /** + * equals + */ + bool equals(const ProjectionFactor&, double tol = 1e-9) const; + + // /** h(x) */ + // Point2 predict(const Pose3& pose, const Point3& point) const { + // return SimpleCamera(*K_, pose).project(point); + // } + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose, const Point3& point, + boost::optional H1, boost::optional H2) const { + SimpleCamera camera(*K_, pose); + if (H1) *H1=Dproject_pose(camera,point); + if (H2) *H2=Dproject_point(camera,point); + Point2 reprojectionError(project(camera, point) - z_); + return reprojectionError.vector(); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + //ar & BOOST_SERIALIZATION_NVP(key1_); + //ar & BOOST_SERIALIZATION_NVP(key2_); + ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(K_); + } + }; + + + // /** + // * Non-linear factor graph for visual SLAM + // */ + // class VSLAMGraph : public NonlinearFactorGraph{ + // + // public: + // + // /** default constructor is empty graph */ + // VSLAMGraph() {} + // + // /** + // * print out graph + // */ + // void print(const std::string& s = "") const { + // NonlinearFactorGraph::print(s); + // } + // + // /** + // * equals + // */ + // bool equals(const VSLAMGraph& p, double tol=1e-9) const { + // return NonlinearFactorGraph::equals(p, tol); + // } + // + // /** + // * Add a constraint on a landmark (for now, *must* be satisfied in any Config) + // * @param j index of landmark + // * @param p to which point to constrain it to + // */ + // void addLandmarkConstraint(int j, const Point3& p = Point3()); + // + // /** + // * Add a constraint on a camera (for now, *must* be satisfied in any Config) + // * @param j index of camera + // * @param p to which pose to constrain it to + // */ + // void addCameraConstraint(int j, const Pose3& p = Pose3()); + // + // private: + // /** Serialization function */ + // friend class boost::serialization::access; + // template + // void serialize(Archive & ar, const unsigned int version) {} + // }; + +} } // namespaces