diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 04eaa0b19..91411a171 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -127,9 +127,11 @@ headers += SQPOptimizer.h SQPOptimizer-inl.h headers += NonlinearConstraint.h NonlinearConstraint-inl.h headers += NonlinearEquality.h headers += Pose2Config.h Pose2Factor.h +headers += UrbanGraph.h sources += NonlinearFactor.cpp sources += Pose2Graph.cpp -check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor testPose2Graph +sources += UrbanConfig.cpp UrbanMeasurement.cpp +check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor testPose2Graph testUrbanGraph testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_LDADD = libgtsam.la testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp @@ -148,6 +150,8 @@ testPose2Factor_SOURCES = $(example) testPose2Factor.cpp testPose2Factor_LDADD = libgtsam.la testPose2Graph_SOURCES = $(example) testPose2Graph.cpp testPose2Graph_LDADD = libgtsam.la +testUrbanGraph_SOURCES = $(example) testUrbanGraph.cpp +testUrbanGraph_LDADD = libgtsam.la # geometry sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 0d3b4fc1f..30326c05c 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -101,25 +101,44 @@ Vector hPose (const Vector& x) { */ /* ************************************************************************* */ Matrix DhPose(const Vector& x) { - Matrix H = eye(6,6); - return H; + Matrix H = eye(6,6); + return H; } /* ************************************************************************* */ Pose3 Pose3::inverse() const -{ - Rot3 Rt = R_.inverse(); - return Pose3(Rt,-(Rt*t_)); -} + { + Rot3 Rt = R_.inverse(); + return Pose3(Rt,-(Rt*t_)); + } /* ************************************************************************* */ Pose3 Pose3::transformPose_to(const Pose3& pose) const -{ - Rot3 cRv = R_ * Rot3(pose.R_.inverse()); - Point3 t = transform_to(pose, t_); - - return Pose3(cRv, t); -} + { + Rot3 cRv = R_ * Rot3(pose.R_.inverse()); + Point3 t = transform_to(pose, t_); + + return Pose3(cRv, t); + } /* ************************************************************************* */ + +Pose3 between(const Pose3& p1, const Pose3& p2){ + Pose3 p; + return p; + // TODO: implement +} +/* ************************************************************************* */ +Matrix Dbetween1(const Pose3& p1, const Pose3& p2){ + Matrix m; + return m; + // TODO: implement +} +/* ************************************************************************* */ +Matrix Dbetween2(const Pose3& p1, const Pose3& p2){ + Matrix m; + return m; + // TODO: implement +} +/* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Pose3.h b/cpp/Pose3.h index b4c4bba53..434cfe879 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -77,6 +77,19 @@ namespace gtsam { /** inverse transformation */ Pose3 inverse() const; + // operators + Pose3 operator+(const Pose3& p3) const{ + // TODO implement the operators "+" + Pose3 p; + return p; + }; + + Pose3 operator-(const Pose3& p3) const{ + // TODO implement the operators "-" + Pose3 p; + return p; + }; + /** composition */ inline Pose3 operator*(const Pose3& B) const { return Pose3(R_*B.R_, t_ + R_*B.t_); @@ -123,6 +136,13 @@ namespace gtsam { Matrix Dtransform_to1(const Pose3& pose, const Point3& p); Matrix Dtransform_to2(const Pose3& pose); // does not depend on p ! + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose3 between(const Pose3& p1, const Pose3& p2); + Matrix Dbetween1(const Pose3& p1, const Pose3& p2); + Matrix Dbetween2(const Pose3& p1, const Pose3& p2); + /** direct measurement of a pose */ Vector hPose(const Vector& x); diff --git a/cpp/Pose3Factor.cpp b/cpp/Pose3Factor.cpp new file mode 100644 index 000000000..32fb79780 --- /dev/null +++ b/cpp/Pose3Factor.cpp @@ -0,0 +1,87 @@ +/** + * @file UrbanOdometryFactor.cpp + * @brief A non-linear factor specialized to the Visual SLAM problem + * @author Frank Dellaert + * @author Viorela Ila + */ + +#include "UrbanConfig.h" +#include "Pose3Factor.h" +#include "Pose3.h" +#include "SimpleCamera.h" + +using namespace std; +namespace gtsam{ + +/* ************************************************************************* */ +Pose3Factor::Pose3Factor() { + /// Arbitrary values + robotPoseNumber_ = 1; + robotPoseName_ = symbol('x',robotPoseNumber_); + keys_.push_back(robotPoseName_); +} +/* ************************************************************************* */ +Pose3Factor::Pose3Factor(const Point2& z, double sigma, int cn, int ln) +: NonlinearFactor(z.vector(), sigma) + { + robotPoseNumber_ = cn; + landmarkNumber_ = ln; + robotPoseName_ = symbol('x',robotPoseNumber_); + landmarkName_ = symbol('l',landmarkNumber_); + keys_.push_back(robotPoseName_); + keys_.push_back(landmarkName_); + } + +/* ************************************************************************* */ +void Pose3Factor::print(const std::string& s) const { + printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(), landmarkName_.c_str()); + gtsam::print(this->z_, s+".z"); +} + +/* ************************************************************************* */ +bool Pose3Factor::equals(const Pose3Factor& p, double tol) const { + if (&p == NULL) return false; + if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_ != p.landmarkNumber_) return false; + if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false; + return true; +} + +// TODO Implement transformPoint2_from +// the difference here that we have a 2d point in a 3D coordinate +Vector Pose3Factor::predict(const Pose3Config& c) const { + Pose3 pose = c.cameraPose(robotPoseNumber_); + Point2 landmark = c.landmarkPoint(landmarkNumber_); + return transformPoint2_from(SimpleCamera(*K_,pose), landmark).vector(); +} + +/* ************************************************************************* */ +Vector Pose3Factor::error_vector(const Pose3Config& c) const { + // Right-hand-side b = (z - h(x))/sigma + Point2 h = predict(c); + return (this->z_ - h); +} + +GaussianFactor::shared_ptr Pose3Factor::linearize(const Pose3Config& c) const +{ + // get arguments from config + Pose3 pose = c.cameraPose(robotPoseNumber_); + Point3 landmark = c.landmarkPoint(landmarkNumber_); + + SimpleCamera camera(*K_,pose); + + // Jacobians + Matrix Dh1 = Dproject_pose(camera, landmark); + Matrix Dh2 = Dproject_point(camera, landmark); + + // Right-hand-side b = (z - h(x)) + Vector h = project(camera, landmark).vector(); + Vector b = this->z_ - h; + + // Make new linearfactor, divide by sigma + GaussianFactor::shared_ptr + p(new GaussianFactor(robotPoseName_, Dh1, landmarkName_, Dh2, b, this->sigma_)); + return p; +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/cpp/Pose3Factor.h b/cpp/Pose3Factor.h new file mode 100644 index 000000000..1c83753c6 --- /dev/null +++ b/cpp/Pose3Factor.h @@ -0,0 +1,71 @@ +/** + * @file Pose3Factor.h + * @brief A Nonlinear Factor, specialized for Urban application + * @author Frank Dellaert + * @author Viorela Ila + */ + +#pragma once + +#include +#include "NonlinearFactor.h" +#include "GaussianFactor.h" +#include "VectorConfig.h" +#include "Pose3.h" +#include "Matrix.h" +#include "ostream" + +namespace gtsam { + +template +class Pose3Factor : public NonlinearFactor { +private: + std::string key1_, key2_; /** The keys of the two poses, order matters */ + Pose3 measured_; + Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ + +public: + + typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor + + Pose3Factor(const std::string& key1, const std::string& key2, + const Pose3& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) { + square_root_inverse_covariance_ = inverse_square_root(measurement_covariance); + } + + /** implement functions needed for Testable */ + void print(const std::string& name) const { + std::cout << name << std::endl; + std::cout << "Factor "<< std::endl; + std::cout << "key1 "<< key1_<& expected, double tol) const {return false;} + + /** implement functions needed to derive from Factor */ + Vector error_vector(const Config& config) const { + //z-h + Pose3 p1 = config.get(key1_), p2 = config.get(key2_); + return (measured_ - between(p1,p2)).vector(); + } + + std::list keys() const { std::list l; return l; } + std::size_t size() const { return 2;} + + /** linearize */ + boost::shared_ptr linearize(const Config& config) const { + Pose3 p1 = config.get(key1_), p2 = config.get(key2_); + Vector b = (measured_ - between(p1,p2)).vector(); + Matrix H1 = Dbetween1(p1,p2); + Matrix H2 = Dbetween2(p1,p2); + boost::shared_ptr linearized(new GaussianFactor( + key1_, square_root_inverse_covariance_ * H1, + key2_, square_root_inverse_covariance_ * H2, + b, 1.0)); + return linearized; + } +}; +} /// namespace gtsam + diff --git a/cpp/UrbanConfig.cpp b/cpp/UrbanConfig.cpp new file mode 100644 index 000000000..62227a48a --- /dev/null +++ b/cpp/UrbanConfig.cpp @@ -0,0 +1,121 @@ +/** + * @file UrbanConfig.cpp + * @brief The Config for Urban example + * @author Frank Dellaert + * @author Viorela Ila + */ + +#include + +#include +#include + +#include "UrbanConfig.h" + +using namespace std; + +// trick from some reading group +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) + +#define SIGMA 1.0 + +namespace gtsam { + +/* ************************************************************************* */ +// Exponential map +UrbanConfig UrbanConfig::exmap(const VectorConfig & delta) const { + + UrbanConfig newConfig; + + for (map::const_iterator it = delta.begin(); it + != delta.end(); it++) { + string key = it->first; + if (key[0] == 'x') { + int robotNumber = atoi(key.substr(1, key.size() - 1).c_str()); + if (robotPoseExists(robotNumber)) { + Pose3 basePose = robotPose(robotNumber); + newConfig.addRobotPose(robotNumber, basePose.exmap(it->second)); + } + } + if (key[0] == 'l') { + int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str()); + if (landmarkPointExists(landmarkNumber)) { + Point2 basePoint = landmarkPoint(landmarkNumber); + newConfig.addLandmark(landmarkNumber, basePoint.exmap(it->second)); + } + } + } + + return newConfig; +} + +/* ************************************************************************* */ +void UrbanConfig::print(const std::string& s) const +{ + printf("%s:\n", s.c_str()); + printf("robot Poses:\n"); + for(PoseMap::const_iterator it = robotIteratorBegin(); it != robotIteratorEnd(); it++) + { + printf("x%d:\n", it->first); + it->second.print(); + } + printf("Landmark Points:\n"); + for(PointMap::const_iterator it = landmarkIteratorBegin(); it != landmarkIteratorEnd(); it++) + { + printf("l%d:\n", (*it).first); + (*it).second.print(); + } +} + +/* ************************************************************************* */ +bool UrbanConfig::equals(const UrbanConfig& c, double tol) const { + for (PoseMap::const_iterator it = robotIteratorBegin(); it + != robotIteratorEnd(); it++) { + if (!c.robotPoseExists(it->first)) return false; + if (!it->second.equals(c.robotPose(it->first), tol)) return false; + } + + for (PointMap::const_iterator it = landmarkIteratorBegin(); it + != landmarkIteratorEnd(); it++) { + if (!c.landmarkPointExists(it->first)) return false; + if (!it->second.equals(c.landmarkPoint(it->first), tol)) return false; + } + return true; +} + +/* ************************************************************************* */ +void UrbanConfig::addRobotPose(const int i, Pose3 cp) +{ + pair robot; + robot.first = i; + robot.second = cp; + robotPoses_.insert(robot); +} + +/* ************************************************************************* */ +void UrbanConfig::addLandmark(const int i, Point2 lp) +{ + pair landmark; + landmark.first = i; + landmark.second = lp; + landmarkPoints_.insert(landmark); +} + +/* ************************************************************************* */ +void UrbanConfig::removeRobotPose(const int i) +{ + if(robotPoseExists(i)) + robotPoses_.erase(i); +} + +/* ************************************************************************* */ +void UrbanConfig::removeLandmark(const int i) +{ + if(landmarkPointExists(i)) + landmarkPoints_.erase(i); +} + +/* ************************************************************************* */ + +} // namespace gtsam + diff --git a/cpp/UrbanConfig.h b/cpp/UrbanConfig.h new file mode 100644 index 000000000..63c2b13f2 --- /dev/null +++ b/cpp/UrbanConfig.h @@ -0,0 +1,121 @@ +/** + * @file UrbanConfig.h + * @brief Config for Urban application + * @author Frank Dellaert + * @author Viorela Ila + */ + +#include +#include +#include +#include +#include "VectorConfig.h" +#include "Pose3.h" +#include "Point2.h" +#include "Testable.h" + +#pragma once + +namespace gtsam{ + +/** + * Config that knows about points and poses + */ +class UrbanConfig : Testable { + + private: + typedef std::map PoseMap; + typedef std::map PointMap; + PointMap landmarkPoints_; + PoseMap robotPoses_; + + public: + typedef std::map::const_iterator const_iterator; + typedef PoseMap::const_iterator const_Pose_iterator; + typedef PointMap::const_iterator const_Point_iterator; + /** + * default constructor + */ + UrbanConfig() {} + + /* + * copy constructor + */ + UrbanConfig(const UrbanConfig& original): + robotPoses_(original.robotPoses_), landmarkPoints_(original.landmarkPoints_){} + + /** + * Exponential map: takes 6D vectors in VectorConfig + * and applies them to the poses in the UrbanConfig. + * Needed for use in nonlinear optimization + */ + UrbanConfig exmap(const VectorConfig & delta) const; + + PoseMap::const_iterator robotIteratorBegin() const { return robotPoses_.begin();} + PoseMap::const_iterator robotIteratorEnd() const { return robotPoses_.end();} + PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints_.begin();} + PointMap::const_iterator landmarkIteratorEnd() const { return landmarkPoints_.end();} + + /** + * print + */ + void print(const std::string& s = "") const; + + /** + * Retrieve robot pose + */ + bool robotPoseExists(int i) const + { + PoseMap::const_iterator it = robotPoses_.find(i); + if (it==robotPoses_.end()) + return false; + return true; + } + + Pose3 robotPose(int i) const { + PoseMap::const_iterator it = robotPoses_.find(i); + if (it==robotPoses_.end()) + throw(std::invalid_argument("robotPose: invalid key")); + return it->second; + } + + /** + * Check whether a landmark point exists + */ + bool landmarkPointExists(int i) const + { + PointMap::const_iterator it = landmarkPoints_.find(i); + if (it==landmarkPoints_.end()) + return false; + return true; + } + + /** + * Retrieve landmark point + */ + Point2 landmarkPoint(int i) const { + PointMap::const_iterator it = landmarkPoints_.find(i); + if (it==landmarkPoints_.end()) + throw(std::invalid_argument("markerPose: invalid key")); + return it->second; + } + + /** + * check whether two configs are equal + */ + bool equals(const UrbanConfig& c, double tol=1e-6) const; + void addRobotPose(const int i, Pose3 cp); + void addLandmark(const int i, Point2 lp); + + void removeRobotPose(const int i); + void removeLandmark(const int i); + + void clear() {landmarkPoints_.clear(); robotPoses_.clear();} + + inline size_t size(){ + return landmarkPoints_.size() + robotPoses_.size(); + } +}; + +} // namespace gtsam + diff --git a/cpp/UrbanGraph.h b/cpp/UrbanGraph.h new file mode 100644 index 000000000..d3fe70a1c --- /dev/null +++ b/cpp/UrbanGraph.h @@ -0,0 +1,69 @@ +/** + * @file UrbanGraph.h + * @brief A factor graph for the Urban problem + * @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" + +namespace gtsam{ + +/** + * Non-linear factor graph for visual SLAM + */ +class UrbanGraph : public gtsam::NonlinearFactorGraph{ + +public: + + /** default constructor is empty graph */ + UrbanGraph() {} + + /** + * print out graph + */ + void print(const std::string& s = "") const { + gtsam::NonlinearFactorGraph::print(s); + } + + /** + * 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) {}; + + /** + * Add an odometry constraint + */ + void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int p ) {}; + + /** + * 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/UrbanMeasurement.cpp b/cpp/UrbanMeasurement.cpp new file mode 100644 index 000000000..ee0721c5e --- /dev/null +++ b/cpp/UrbanMeasurement.cpp @@ -0,0 +1,76 @@ +/** + * @file UrbanMeasurement.cpp + * @brief A non-linear factor specialized to the Visual SLAM problem + * @author Frank Dellaert + * @author Viorela Ila + */ + +#include "UrbanConfig.h" +#include "UrbanMeasurement.h" +#include "Pose3.h" +#include "SimpleCamera.h" + +using namespace std; +namespace gtsam{ +/** receives the 2D point in world coordinates and transforms it to Pose coordinates */ +Point3 transformPoint2_to(const Pose3& pose, const Point2& p); +Matrix DtransformPoint2_to1(const Pose3& pose, const Point2& p); +Matrix DtransformPoint2_to2(const Pose3& pose); // does not depend on p ! +/* ************************************************************************* */ +UrbanMeasurement::UrbanMeasurement() { + /// Arbitrary values + robotPoseNumber_ = 111; + landmarkNumber_ = 222; + robotPoseName_ = symbol('x',robotPoseNumber_); + landmarkName_ = symbol('l',landmarkNumber_); + keys_.push_back(robotPoseName_); + keys_.push_back(landmarkName_); +} +/* ************************************************************************* */ +UrbanMeasurement::UrbanMeasurement(const Point2& z, double sigma, int cn, int ln) +: NonlinearFactor(z.vector(), sigma) + { + robotPoseNumber_ = cn; + landmarkNumber_ = ln; + robotPoseName_ = symbol('x',robotPoseNumber_); + landmarkName_ = symbol('l',landmarkNumber_); + keys_.push_back(robotPoseName_); + keys_.push_back(landmarkName_); + } + +/* ************************************************************************* */ +void UrbanMeasurement::print(const std::string& s) const { + printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(), landmarkName_.c_str()); + gtsam::print(this->z_, s+".z"); +} + +/* ************************************************************************* */ +bool UrbanMeasurement::equals(const UrbanMeasurement& p, double tol) const { + if (&p == NULL) return false; + if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_ != p.landmarkNumber_) return false; + if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false; + return true; +} + + +// the difference here that we have a 2d point in a 3D coordinate +Vector UrbanMeasurement::predict(const UrbanConfig& c) const { + Pose3 pose = c.robotPose(robotPoseNumber_); + Point2 landmark = c.landmarkPoint(landmarkNumber_); + // TODO Implement predict function + Vector v; + return v; +} + +/* ************************************************************************* */ +Vector UrbanMeasurement::error_vector(const UrbanConfig& c) const { + // Right-hand-side b = (z - h(x))/sigma + Point2 h = predict(c); + // TODO Return z-h(x) + Vector v; + return v; +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/cpp/UrbanMeasurement.h b/cpp/UrbanMeasurement.h new file mode 100644 index 000000000..1612e37ad --- /dev/null +++ b/cpp/UrbanMeasurement.h @@ -0,0 +1,100 @@ +/** + * @file UrbanMeasurement.h + * @brief A Nonlinear Factor, specialized for Urban application + * @author Frank Dellaert + * @author Viorela Ila + */ + +#pragma once + +#include "NonlinearFactor.h" +#include "GaussianFactor.h" +#include "Testable.h" +#include "Point2.h" + +namespace gtsam { + +class UrbanConfig; + +/** + * Non-linear factor for a constraint derived from a 2D measurement, + */ +class UrbanMeasurement : public NonlinearFactor, Testable +{ +private: + + int robotPoseNumber_, landmarkNumber_; + std::string robotPoseName_, landmarkName_; + //boost::shared_ptr K_; // Calibration stored in each factor. FD: need to think about this. + typedef NonlinearFactor ConvenientFactor; + +public: + + typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor + //typedef boost::shared_ptr shared_ptrK; + + /** + * Default constructor + */ + UrbanMeasurement(); + + /** + * Constructor + * @param z is the 2 dimensional location of landmark respect to the robot (the measurement) + * @param sigma is the standard deviation + * @param robotPoseNumber is basically the pose number + * @param landmarkNumber is the index of the landmark + * + * + */ + UrbanMeasurement(const Point2& z, double sigma, int robotPoseNumber, int landmarkNumber); + + + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s="UrbanMeasurement") const; + + /** + * equals + */ + bool equals(const UrbanMeasurement&, double tol=1e-9) const; + + /** + * predict the measurement (is that update ??) + */ + Vector predict(const UrbanConfig&) const; + + /** + * calculate the error of the factor + */ + Vector error_vector(const UrbanConfig&) const; + + /** + * linerarization + */ + + GaussianFactor::shared_ptr linearize(const UrbanConfig&) const { + //TODO implement linearize + + }; + + int getrobotPoseNumber() const { return robotPoseNumber_; } + int getLandmarkNumber() const { return landmarkNumber_; } + + + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(robotPoseNumber_); + ar & BOOST_SERIALIZATION_NVP(landmarkNumber_); + ar & BOOST_SERIALIZATION_NVP(robotPoseName_); + ar & BOOST_SERIALIZATION_NVP(landmarkName_); + } +}; + +} diff --git a/cpp/testUrbanGraph b/cpp/testUrbanGraph new file mode 100755 index 000000000..b212fe436 --- /dev/null +++ b/cpp/testUrbanGraph @@ -0,0 +1,99 @@ +#! /bin/sh + +# testUrbanGraph - temporary wrapper script for .libs/testUrbanGraph +# Generated by ltmain.sh - GNU libtool 1.5.22 (1.1220.2.365 2005/12/18 22:14:06) +# +# The testUrbanGraph program cannot be directly executed until all the libtool +# libraries that it depends on are installed. +# +# This wrapper script should never be moved out of the build directory. +# If it is, it will not operate correctly. + +# Sed substitution that helps us do robust quoting. It backslashifies +# metacharacters that are still active within double-quoted strings. +Xsed='/usr/bin/sed -e 1s/^X//' +sed_quote_subst='s/\([\\`\\"$\\\\]\)/\\\1/g' + +# The HP-UX ksh and POSIX shell print the target directory to stdout +# if CDPATH is set. +(unset CDPATH) >/dev/null 2>&1 && unset CDPATH + +relink_command="" + +# This environment variable determines our operation mode. +if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then + # install mode needs the following variable: + notinst_deplibs=' libgtsam.la' +else + # When we are sourced in execute mode, $file and $echo are already set. + if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then + echo="/bin/echo" + file="$0" + # Make sure echo works. + if test "X$1" = X--no-reexec; then + # Discard the --no-reexec flag, and continue. + shift + elif test "X`($echo '\t') 2>/dev/null`" = 'X\t'; then + # Yippee, $echo works! + : + else + # Restart under the correct shell, and then maybe $echo will work. + exec /bin/sh "$0" --no-reexec ${1+"$@"} + fi + fi + + # Find the directory that this script lives in. + thisdir=`$echo "X$file" | $Xsed -e 's%/[^/]*$%%'` + test "x$thisdir" = "x$file" && thisdir=. + + # Follow symbolic links until we get to the real thisdir. + file=`ls -ld "$file" | /usr/bin/sed -n 's/.*-> //p'` + while test -n "$file"; do + destdir=`$echo "X$file" | $Xsed -e 's%/[^/]*$%%'` + + # If there was a directory component, then change thisdir. + if test "x$destdir" != "x$file"; then + case "$destdir" in + [\\/]* | [A-Za-z]:[\\/]*) thisdir="$destdir" ;; + *) thisdir="$thisdir/$destdir" ;; + esac + fi + + file=`$echo "X$file" | $Xsed -e 's%^.*/%%'` + file=`ls -ld "$thisdir/$file" | /usr/bin/sed -n 's/.*-> //p'` + done + + # Try to get the absolute directory name. + absdir=`cd "$thisdir" && pwd` + test -n "$absdir" && thisdir="$absdir" + + program='testUrbanGraph' + progdir="$thisdir/.libs" + + + if test -f "$progdir/$program"; then + # Add our own library path to DYLD_LIBRARY_PATH + DYLD_LIBRARY_PATH="/Users/vila/borg/gtsam/cpp/.libs:$DYLD_LIBRARY_PATH" + + # Some systems cannot cope with colon-terminated DYLD_LIBRARY_PATH + # The second colon is a workaround for a bug in BeOS R4 sed + DYLD_LIBRARY_PATH=`$echo "X$DYLD_LIBRARY_PATH" | $Xsed -e 's/::*$//'` + + export DYLD_LIBRARY_PATH + + if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then + # Run the actual program with our arguments. + + exec "$progdir/$program" ${1+"$@"} + + $echo "$0: cannot exec $program ${1+"$@"}" + exit 1 + fi + else + # The program doesn't exist. + $echo "$0: error: \`$progdir/$program' does not exist" 1>&2 + $echo "This script is just a wrapper for $program." 1>&2 + /bin/echo "See the libtool documentation for more information." 1>&2 + exit 1 + fi +fi diff --git a/cpp/testUrbanGraph.cpp b/cpp/testUrbanGraph.cpp new file mode 100644 index 000000000..008dde90f --- /dev/null +++ b/cpp/testUrbanGraph.cpp @@ -0,0 +1,108 @@ +/** + * @file testUrbanGraph.cpp + * @brief Unit test for two robot poses and four landmarks + * @author Frank Dellaert + * @author Viorela Ila + */ +#include +#include // for operator += +using namespace boost::assign; + +#include + +#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; + +/* ************************************************************************* */ + +Point2 landmark1( 2, 5); +Point2 landmark2( 2, 10); +Point2 landmark3( -2, 5); +Point2 landmark4( -2,-10); + +/* Robot is at (0,0,0) looking in global "y" direction. + * For the local frame we used Navlab convention, + * x-looks forward, y-looks right, z- down*/ +Pose3 robotPose1(Matrix_(3,3, + 0., 1., 0., + 1., 0., 0., + 0., 0.,-1. + ), + Point3(0,0,0)); + +/* move at a speed 10 m/s (36 km/h or 22 mph), 10 Hz update, we move 1m.*/ +Pose3 robotPose2(Matrix_(3,3, + 0., 1., 0., + 1., 0., 0., + 0., 0.,-1. + ), + Point3(0,1,0)); + +/* ************************************************************************* */ +/* the measurements are relative to the robot pose so + * they are in robot coordinate frame different from the global (above)*/ +UrbanGraph testGraph() { + + double sigma = 0.2;// 20 cm + double sigmadx = 0.01; // 1 cm + double sigmayaw = M_PI/180.0;// 1 degree + + UrbanGraph g; + g.addOriginConstraint(1); // pose1 is the origin + g.addMeasurement( 5, 2, sigma, 1, 1); // z11 + g.addMeasurement(10, 2, sigma, 1, 2); // z12 + g.addMeasurement( 5, -2, sigma, 1, 3); // z13 + g.addMeasurement(10, -2, sigma, 1, 4); // z14 + g.addOdometry(1, 0, sigmadx,sigmayaw, 1); // 1m forward, 0 yaw + g.addMeasurement( 4, 2, sigma, 2, 1); // z21 + g.addMeasurement( 9, 2, sigma, 2, 2); // z22 + g.addMeasurement( 4, -2, sigma, 2, 3); // z23 + g.addMeasurement( 9, -2, sigma, 2, 4); // z24 + + return g; +} + +/* ************************************************************************* */ +TEST( UrbanGraph, optimizeLM) +{ + // build a graph + UrbanGraph graph = testGraph(); + + // Create an initial configuration corresponding to the ground truth + boost::shared_ptr initialEstimate(new UrbanConfig); + initialEstimate->addRobotPose(1, robotPose1); + initialEstimate->addRobotPose(2, robotPose2); + initialEstimate->addLandmark(1, landmark1); + initialEstimate->addLandmark(2, landmark2); + initialEstimate->addLandmark(3, landmark3); + initialEstimate->addLandmark(4, landmark4); + + // Create an ordering of the variables + Ordering ordering; + ordering += "l1","l2","l3","l4","x1","x2"; // boost assign syntax + + // Create an optimizer and check its error + // We expect the initial to be zero because config is the ground truth + //Optimizer optimizer(graph, ordering, initialEstimate, 1e-5); + //DOUBLES_EQUAL(0, optimizer.error(), 1e-9); + + // Iterate once, and the config should not have changed because we started + // with the ground truth + + // Optimizer afterOneIteration = optimizer.iterate(); + // DOUBLES_EQUAL(0, optimizer.error(), 1e-9); + + // check if correct + // CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config()))); +} + +/* ************************************************************************* */ +int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0;} +/* ************************************************************************* */