diff --git a/cpp/Makefile.am b/cpp/Makefile.am index f789de37b..487fdffe5 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -189,8 +189,7 @@ testPose3Factor_SOURCES = $(example) testPose3Factor.cpp testPose3Factor_LDADD = libgtsam.la # Urban -headers += UrbanGraph.h -sources += UrbanConfig.cpp UrbanMeasurement.cpp +sources += UrbanFactor.cpp UrbanConfig.cpp UrbanMeasurement.cpp UrbanGraph.cpp check_PROGRAMS += testUrbanConfig testUrbanMeasurement testUrbanGraph testUrbanConfig_SOURCES = $(example) testUrbanConfig.cpp testUrbanConfig_LDADD = libgtsam.la diff --git a/cpp/UrbanConfig.h b/cpp/UrbanConfig.h index 63c2b13f2..fff15dbc5 100644 --- a/cpp/UrbanConfig.h +++ b/cpp/UrbanConfig.h @@ -5,14 +5,11 @@ * @author Viorela Ila */ -#include #include -#include -#include -#include "VectorConfig.h" -#include "Pose3.h" -#include "Point2.h" #include "Testable.h" +#include "VectorConfig.h" +#include "Point2.h" +#include "Pose3.h" #pragma once diff --git a/cpp/UrbanFactor.cpp b/cpp/UrbanFactor.cpp new file mode 100644 index 000000000..7b96789ba --- /dev/null +++ b/cpp/UrbanFactor.cpp @@ -0,0 +1,21 @@ +/* + * UrbanFactor.cpp + * + * Created on: Dec 17, 2009 + * Author: Frank Dellaert + */ + +#include "UrbanFactor.h" + +namespace gtsam { + + UrbanFactor::UrbanFactor() { + // TODO Auto-generated constructor stub + + } + + UrbanFactor::~UrbanFactor() { + // TODO Auto-generated destructor stub + } + +} diff --git a/cpp/UrbanFactor.h b/cpp/UrbanFactor.h new file mode 100644 index 000000000..f2f59a3a4 --- /dev/null +++ b/cpp/UrbanFactor.h @@ -0,0 +1,34 @@ +/* + * UrbanFactor.h + * + * Created on: Dec 17, 2009 + * Author: Frank Dellaert + */ + +#ifndef URBANFACTOR_H_ +#define URBANFACTOR_H_ + +#include "NonlinearFactor.h" +#include "UrbanConfig.h" + +namespace gtsam { + + class UrbanFactor : public NonlinearFactor { + public: + UrbanFactor(); + virtual ~UrbanFactor(); + + /** Vector of errors */ + Vector error_vector(const UrbanConfig& c) const { return zero(0); } + + /** linearize to a GaussianFactor */ + boost::shared_ptr linearize(const UrbanConfig& c) const { + boost::shared_ptr factor(new GaussianFactor); + return factor; + } + + }; + +} + +#endif /* URBANFACTOR_H_ */ diff --git a/cpp/UrbanGraph.cpp b/cpp/UrbanGraph.cpp new file mode 100644 index 000000000..c6c8d3188 --- /dev/null +++ b/cpp/UrbanGraph.cpp @@ -0,0 +1,54 @@ +/** + * @file UrbanGraph.cpp + * @brief A factor graph for the Urban problem + * @author Frank Dellaert + * @author Viorela Ila + */ + +#include "FactorGraph-inl.h" +#include "NonlinearOptimizer-inl.h" +#include "UrbanGraph.h" + +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; + + /* ************************************************************************* */ + UrbanGraph::UrbanGraph() { + } + + /* ************************************************************************* */ + void UrbanGraph::print(const std::string& s) const { + gtsam::NonlinearFactorGraph::print(s); + // TODO + } + + /* ************************************************************************* */ + bool UrbanGraph::equals(const UrbanGraph& p, double tol) const { + return gtsam::NonlinearFactorGraph::equals(p, tol); + // TODO + } + + /* ************************************************************************* */ + void UrbanGraph::addMeasurement(double x, double y, double sigma, int p1, + int p2) { + // TODO + } + ; + + /* ************************************************************************* */ + void UrbanGraph::addOdometry(double dx, double yaw, double sigmadx, + double sigmayaw, int p) { + // TODO + } + + /* ************************************************************************* */ + void UrbanGraph::addOriginConstraint(int p) { + // TODO + } + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/cpp/UrbanGraph.h b/cpp/UrbanGraph.h index d3fe70a1c..aa0d7fe6d 100644 --- a/cpp/UrbanGraph.h +++ b/cpp/UrbanGraph.h @@ -4,66 +4,56 @@ * @author Frank Dellaert * @author Viorela Ila */ + #pragma once -#include -#include -#include -#include - #include "NonlinearFactorGraph.h" -#include "FactorGraph-inl.h" -#include "UrbanMeasurement.h" -#include "Pose3Factor.h" -#include "UrbanConfig.h" -#include "Testable.h" +#include "UrbanFactor.h" -namespace gtsam{ +namespace gtsam { -/** - * Non-linear factor graph for visual SLAM - */ -class UrbanGraph : public gtsam::NonlinearFactorGraph{ + /** + * Non-linear factor graph for visual SLAM + */ + class UrbanGraph: public gtsam::NonlinearFactorGraph { -public: + public: - /** default constructor is empty graph */ - UrbanGraph() {} + /** default constructor is empty graph */ + UrbanGraph(); - /** - * print out graph - */ - void print(const std::string& s = "") const { - gtsam::NonlinearFactorGraph::print(s); - } + /** + * print out graph + */ + void print(const std::string& s = "") const; - /** - * equals - */ - bool equals(const UrbanGraph& p, double tol=1e-9) const { - return gtsam::NonlinearFactorGraph::equals(p, tol); - } - // TODO implement addMeasurenment, addOdometry and addOriginalConstraint (out of the class) - /** - * Add a landmark constraint - */ - void addMeasurement(double x, double y, double sigma, int p1, int p2) {}; + /** + * equals + */ + bool equals(const UrbanGraph& p, double tol = 1e-9) const; - /** - * Add an odometry constraint - */ - void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int p ) {}; + /** + * Add a landmark constraint + */ + void addMeasurement(double x, double y, double sigma, int p1, int p2); - /** - * Add an initial constraint on the first pose - */ - void addOriginConstraint(int p){}; + /** + * Add an odometry constraint + */ + void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, + int p); -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) {} -}; + /** + * Add an initial constraint on the first pose + */ + void addOriginConstraint(int p); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + } + }; } // namespace gtsam diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp index da034e939..9c180e717 100644 --- a/cpp/VSLAMGraph.cpp +++ b/cpp/VSLAMGraph.cpp @@ -23,9 +23,6 @@ template class FactorGraph; template class NonlinearFactorGraph; template class NonlinearOptimizer; -/* ************************************************************************* */ - - /* ************************************************************************* */ bool compareLandmark(const std::string& key, const VSLAMConfig& feasible, diff --git a/cpp/testUrbanGraph.cpp b/cpp/testUrbanGraph.cpp index 008dde90f..5a55a068a 100644 --- a/cpp/testUrbanGraph.cpp +++ b/cpp/testUrbanGraph.cpp @@ -13,9 +13,8 @@ using namespace boost::assign; #include "UrbanGraph.h" #include "NonlinearFactorGraph-inl.h" #include "NonlinearOptimizer-inl.h" -#include "Pose3.h" -#include "Point2.h" #include "Ordering.h" + using namespace std; using namespace gtsam; typedef NonlinearOptimizer Optimizer;