diff --git a/cpp/Makefile.am b/cpp/Makefile.am index f48d2acab..933d92e19 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -199,18 +199,6 @@ testPose2Graph_LDADD = libgtsam.la testPose3Factor_SOURCES = $(example) testPose3Factor.cpp testPose3Factor_LDADD = libgtsam.la -# Urban -#sources += UrbanFactor.cpp UrbanConfig.cpp UrbanMeasurement.cpp UrbanOdometry.cpp UrbanGraph.cpp -#check_PROGRAMS += testUrbanConfig testUrbanMeasurement testUrbanOdometry testUrbanGraph -#testUrbanConfig_SOURCES = $(example) testUrbanConfig.cpp -#testUrbanConfig_LDADD = libgtsam.la -#testUrbanMeasurement_SOURCES = $(example) testUrbanMeasurement.cpp -#testUrbanMeasurement_LDADD = libgtsam.la -#testUrbanOdometry_SOURCES = $(example) testUrbanOdometry.cpp -#testUrbanOdometry_LDADD = libgtsam.la -#testUrbanGraph_SOURCES = $(example) testUrbanGraph.cpp -#testUrbanGraph_LDADD = libgtsam.la - # Robot Control example system sources += ControlConfig.cpp ControlPoint.cpp ControlGraph.cpp check_PROGRAMS += testControlConfig testControlPoint testControlGraph diff --git a/cpp/UrbanConfig.cpp b/cpp/UrbanConfig.cpp deleted file mode 100644 index f62edf79e..000000000 --- a/cpp/UrbanConfig.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/** - * @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) -{ - robotPoses_.insert(make_pair(i,cp)); -} - -/* ************************************************************************* */ -void UrbanConfig::addLandmark(const int j, Point2 lp) -{ - landmarkPoints_.insert(make_pair(j,lp)); -} - -/* ************************************************************************* */ -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 deleted file mode 100644 index adfc9e1eb..000000000 --- a/cpp/UrbanConfig.h +++ /dev/null @@ -1,134 +0,0 @@ -/** - * @file UrbanConfig.h - * @brief Config for Urban application - * @author Frank Dellaert - * @author Viorela Ila - */ - -#include -#include -#include "Testable.h" -#include "VectorConfig.h" -#include "Point2.h" -#include "Pose3.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_; - - static std::string symbol(char c, int index) { - std::stringstream ss; - ss << c << index; - return ss.str(); - } - - 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) : - landmarkPoints_(original.landmarkPoints_), - robotPoses_(original.robotPoses_) - { - } - - /** - * 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); - return (it != robotPoses_.end()); - } - - Pose3 robotPose(int i) const { - PoseMap::const_iterator it = robotPoses_.find(i); - if (it == robotPoses_.end()) throw(std::invalid_argument( - "robotPose: invalid key " + symbol('x',i))); - return it->second; - } - - /** - * Check whether a landmark point exists - */ - bool landmarkPointExists(int i) const { - PointMap::const_iterator it = landmarkPoints_.find(i); - return (it != landmarkPoints_.end()); - } - - /** - * 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 " + symbol('l',i))); - 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/UrbanFactor.cpp b/cpp/UrbanFactor.cpp deleted file mode 100644 index 66a0f130b..000000000 --- a/cpp/UrbanFactor.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/* - * UrbanFactor.cpp - * - * Created on: Dec 17, 2009 - * Author: Frank Dellaert - */ - -#include "UrbanFactor.h" - -namespace gtsam { - - UrbanFactor::UrbanFactor() { - // TODO Auto-generated constructor stub - } - - UrbanFactor::UrbanFactor(const Vector& z, const double sigma) : - NonlinearFactor (z,sigma) { - // TODO - } - - UrbanFactor::~UrbanFactor() { - // TODO Auto-generated destructor stub - } - -} diff --git a/cpp/UrbanFactor.h b/cpp/UrbanFactor.h deleted file mode 100644 index 6fcca91ae..000000000 --- a/cpp/UrbanFactor.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * UrbanFactor.h - * - * Created on: Dec 17, 2009 - * Author: Frank Dellaert - */ - -#ifndef URBANFACTOR_H_ -#define URBANFACTOR_H_ - -#include "NonlinearFactor.h" -#include "UrbanConfig.h" - -namespace gtsam { - - /** - * Base class for UrbanMeasurement and UrbanOdometry - */ - class UrbanFactor: public NonlinearFactor { - public: - - UrbanFactor(); - UrbanFactor(const Vector& z, const double sigma); - virtual ~UrbanFactor(); - }; - -} - -#endif /* URBANFACTOR_H_ */ diff --git a/cpp/UrbanGraph.cpp b/cpp/UrbanGraph.cpp deleted file mode 100644 index 05b756bd8..000000000 --- a/cpp/UrbanGraph.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/** - * @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" -#include "UrbanMeasurement.h" -#include "UrbanOdometry.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(const boost::shared_ptr &sensorMatrix, - double x, double y, double sigma, int i, int j) { - Point2 z(x,y); - sharedFactor factor(new UrbanMeasurement(sensorMatrix, z,sigma,i,j)); - push_back(factor); - } - ; - - /* ************************************************************************* */ - void UrbanGraph::addOdometry(double dx, double yaw, double sigmadx, - double sigmayaw, int p) { - Vector z = Vector_(dx,0,0,yaw,0,0); - Matrix cov = eye(6); - cov(1,1)=sigmadx*sigmadx; - cov(4,4)=sigmadx*sigmadx; - sharedFactor factor(new UrbanOdometry(symbol('x',p),symbol('x',p+1),z,cov)); - push_back(factor); - } - - /* ************************************************************************* */ - void UrbanGraph::addOriginConstraint(int p) { - // TODO - } - -/* ************************************************************************* */ -} // namespace gtsam diff --git a/cpp/UrbanGraph.h b/cpp/UrbanGraph.h deleted file mode 100644 index 590aa500a..000000000 --- a/cpp/UrbanGraph.h +++ /dev/null @@ -1,59 +0,0 @@ -/** - * @file UrbanGraph.h - * @brief A factor graph for the Urban problem - * @author Frank Dellaert - * @author Viorela Ila - */ - -#pragma once - -#include "NonlinearFactorGraph.h" -#include "UrbanFactor.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; - - /** - * equals - */ - bool equals(const UrbanGraph& p, double tol = 1e-9) const; - - /** - * Add a landmark constraint - */ - void addMeasurement(const boost::shared_ptr &sensorMatrix, - double x, double y, double sigma, int i, int j); - - /** - * Add an odometry constraint - */ - void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int i); - - /** - * 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 deleted file mode 100644 index 097c0b3bb..000000000 --- a/cpp/UrbanMeasurement.cpp +++ /dev/null @@ -1,188 +0,0 @@ -/** - * @file UrbanMeasurement.cpp - * @brief A non-linear factor specialized to the Visual SLAM problem - * @author Frank Dellaert - * @author Viorela Ila - */ - -#include "UrbanMeasurement.h" -#include "Pose3.h" - -using namespace std; - -#define _GET_TEMPORARIES \ - -namespace gtsam { - - /* ************************************************************************* */ - - // JC: I'd like to break this out into component calculations, but there's too much - // Clearly there is a lot of code overlap (and redundant calculations) between - // this and the calculation of the relevant jacobians. Need to look at cleaning - // this up when I have a better grasp of the larger implications. I don't think - // there's any reason we can't just merge transform_to and the two derivative - // functions into a single function to remove the overlap. - - - - - UrbanMeasurement::Transform transform_to(const boost::shared_ptr &sensorMatrix, - const Pose3& robotpose, const Point2& lmpos, - bool getJacobians) { - // Notation: - // ph = phi = roll - // th = theta = pitch - // ps = psi = yaw - Vector rpy = RQ(robotpose.rotation().matrix()); - double dx = lmpos.x() - robotpose.translation().x(); - double dy = lmpos.y() - robotpose.translation().y(); - double s23 = (*sensorMatrix)(2, 3); - double cps = cos(rpy[2]); - double sps = sin(rpy[2]); - double secph = 1/cos(rpy[0]); - double tph = tan(rpy[0]); - double sph = sin(rpy[0]); - double sth = sin(rpy[1]); - double cth = cos(rpy[1]); - double tth = tan(rpy[1]); - double secth = 1/cth; - - UrbanMeasurement::Transform ret; - ret.get<0>().reset(new Point2(cth*dy*sps - + (-s23*secph+dy*sps*sth+dx*sps*tph)*tth - + cps*(cth*dx+dx*sth*tth-dy*tph*tth), - secph*(cps*dy+s23*sph-dx*sps))); - - - if (getJacobians) { - ret.get<1>().reset(new Matrix(2, 6)); - Matrix &D1 = *(ret.get<1>()); - D1(0, 0) = (-cps*secth-sps*tph*tth); - D1(0, 1) = (-secth*sps+cps*tph*tth); - D1(0, 2) = 0; - D1(0, 3) = -((secph*secph*(cps*dy+s23*sph-dx*sps)*tth)); - D1(0, 4) = (-((secth*(secth*(s23*secph+(cps*dy-dx*sps)*tph) - +(-cps*dx-dy*sps)*tth)))); - D1(0, 5) = ((cth*(cps*dy-dx*sps)+(cps*(dy*sth+dx*tph) - +sps*(-dx*sth+dy*tph))*tth)); - - D1(1, 0) = secph*sps; - D1(1, 1) = -cps*secph; - D1(1, 2) = 0; - D1(1, 3) = secph*(s23*secph+(cps*dy-dx*sps)*tph); - D1(1, 4) = 0; - D1(1, 5) = secph*(-cps*dx-dy*sps); - - - ret.get<2>().reset(new Matrix(2, 2)); - Matrix &D2 = *(ret.get<2>()); - D2(0, 0) = cps*secth+sps*tph*tth; - D2(0, 1) = secth*sps-cps*tph*tth; - D2(1, 0) = -secph*sps; - D2(1, 1) = cps*secph; - } - return ret; - } - - - /* ************************************************************************* */ - UrbanMeasurement::UrbanMeasurement() { - /// Arbitrary values - - // JC: This should cause use of a default-constructed measurement to segfault. - // Not entirely sure as to the desired behaviour when uninitialized data is used. - // If you're crashing because of this and you didn't expect to, sorry. :) - sensorMatrix_.reset(); - - robotPoseNumber_ = 111; - landmarkNumber_ = 222; - robotPoseName_ = symbol('x', robotPoseNumber_); - landmarkName_ = symbol('l', landmarkNumber_); - keys_.push_back(robotPoseName_); - keys_.push_back(landmarkName_); - } - - /* ************************************************************************* */ - UrbanMeasurement::UrbanMeasurement(const boost::shared_ptr &sensorMatrix, - const Point2& z, double sigma, int i, - int j) : - UrbanFactor(z.vector(), sigma) { - - sensorMatrix_ = sensorMatrix; - robotPoseNumber_ = i; - landmarkNumber_ = j; - 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; - } - - /* ************************************************************************* */ - Vector UrbanMeasurement::error_vector(const UrbanConfig& x) const { - Pose3 pose = x.robotPose(robotPoseNumber_); - Point2 landmark = x.landmarkPoint(landmarkNumber_); - // Right-hand-side b = (z - h(x))/sigma -// fprintf(stderr, "p: %.16g l: %.16g\n", -// pose.rotation().vector()[0], -// landmark.x()); - boost::shared_ptr h = transform_to(sensorMatrix_, pose,landmark, false).get<0>(); -// fprintf(stderr, "H: %.16g %.16g\nz: %.16g %.16g\n", -// h->vector()[0], h->vector()[1], -// z_[0], z_[1]); - return (z_ - h->vector()); - } - - /* ************************************************************************* */ - GaussianFactor::shared_ptr UrbanMeasurement::linearize(const UrbanConfig& config) const { - Pose3 pose = config.robotPose(robotPoseNumber_); - Point2 landmark = config.landmarkPoint(landmarkNumber_); - - // JC: Things I don't yet understand: - // - // What should the keys be? Presumably I just need to match a convention. - // - // Should the jacobians be premultiplied at the time of - // GaussianFactor construction? - // - // Is GaussianFactor copying its constructor arguments? (If not, this is - // not safe, nor is what this code replaced). - // - // Why is sigma a double instead of a matrix? - - - ostringstream k; - k << "pose" << robotPoseNumber_; - string posekey = k.str(); - k.clear(); - k << "lm" << landmarkNumber_; - string lmkey = k.str(); - k.clear(); - - Transform tr = transform_to(sensorMatrix_, pose,landmark, true); - - - return GaussianFactor::shared_ptr( - new GaussianFactor(posekey, - *(tr.get<1>()), - lmkey, - *(tr.get<2>()), - z_ - tr.get<0>()->vector(), - sigma_)); - } -} // namespace gtsam diff --git a/cpp/UrbanMeasurement.h b/cpp/UrbanMeasurement.h deleted file mode 100644 index 4592386e2..000000000 --- a/cpp/UrbanMeasurement.h +++ /dev/null @@ -1,104 +0,0 @@ -/** - * @file UrbanMeasurement.h - * @brief A Nonlinear Factor, specialized for Urban application - * @author Frank Dellaert - * @author Viorela Ila - */ - -#pragma once - -#include "UrbanFactor.h" - -namespace gtsam { - - class UrbanConfig; - - /** - * Non-linear factor for a constraint derived from a 2D measurement, - */ - class UrbanMeasurement: public UrbanFactor { - private: - boost::shared_ptr sensorMatrix_; - 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: - // Return type for the transform_to function - typedef boost::tuple, - boost::shared_ptr, - boost::shared_ptr > Transform; - - 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 boost::shared_ptr &sensorMatrix, - 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; - - /** - * calculate the error of the factor - */ - Vector error_vector(const UrbanConfig&) const; - - /** - * linerarization - */ - - GaussianFactor::shared_ptr linearize(const UrbanConfig&) const; - - 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_); - } - }; - - /** - * Transform 2D landmark into 6D pose, and its derivatives - */ - // JC: These are exported only for testbenches? - UrbanMeasurement::Transform transform_to(const boost::shared_ptr &sensorMatrix, - const Pose3& robotpose, const Point2& lmpos, - bool getJacobians); - -} // namespace gtsam diff --git a/cpp/UrbanOdometry.cpp b/cpp/UrbanOdometry.cpp deleted file mode 100644 index 0db972253..000000000 --- a/cpp/UrbanOdometry.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/** - * @file UrbanOdometry.cpp - * @brief A non-linear factor specialized to the Visual SLAM problem - * @author Frank Dellaert - * @author Viorela Ila - */ - -#include "UrbanOdometry.h" - -using namespace std; -namespace gtsam { - -/* ************************************************************************* * -UrbanOdometry::UrbanOdometry() { - /// Arbitrary values - robotPoseNumber_ = 1; - robotPoseName_ = symbol('x',robotPoseNumber_); - keys_.push_back(robotPoseName_); -} -/* ************************************************************************* * -UrbanOdometry::UrbanOdometry(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 UrbanOdometry::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 UrbanOdometry::equals(const UrbanOdometry& 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 UrbanOdometry::predict(const Pose3Config& c) const { - Pose3 pose = c.cameraPose(robotPoseNumber_); - Point2 landmark = c.landmarkPoint(landmarkNumber_); - return transformPoint2_from(SimpleCamera(*K_,pose), landmark).vector(); -} - -/* ************************************************************************* * -Vector UrbanOdometry::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 UrbanOdometry::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/UrbanOdometry.h b/cpp/UrbanOdometry.h deleted file mode 100644 index c3acbba50..000000000 --- a/cpp/UrbanOdometry.h +++ /dev/null @@ -1,74 +0,0 @@ -/** - * @file UrbanOdometry.h - * @brief A Nonlinear Factor, specialized for Urban application - * @author Frank Dellaert - * @author Viorela Ila - */ - -#pragma once - -#include "UrbanFactor.h" - -namespace gtsam { - - class UrbanOdometry: public UrbanFactor { - private: - std::string key1_, key2_; /** The keys of the two poses, order matters */ - Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ - - public: - - typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor - - UrbanOdometry(const std::string& key1, const std::string& key2, - const Vector& measured, const Matrix& measurement_covariance) : - UrbanFactor(measured, 1.0), - key1_(key1), key2_(key2) { - 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_ << std::endl; - std::cout << "key2 " << key2_ << std::endl; - gtsam::print(z_,"measured "); - gtsam::print(square_root_inverse_covariance_, "square_root_inverse_covariance"); - } - - bool equals(const NonlinearFactor& expected, double tol) const { - return false; - } - - /** implement functions needed to derive from Factor */ - Vector error_vector(const UrbanConfig& config) const { - return zero(6); - } - - std::list keys() const { - std::list l; - l.push_back(key1_); - l.push_back(key2_); - return l; - } - - std::size_t size() const { - return 2; - } - - /** linearize */ - boost::shared_ptr linearize(const UrbanConfig& config) const { - Vector b = zero(6); - Matrix H1 = zeros(6,6); - Matrix H2 = zeros(6,6); - 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/testUrbanConfig.cpp b/cpp/testUrbanConfig.cpp deleted file mode 100644 index ee4e2089a..000000000 --- a/cpp/testUrbanConfig.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * @file testUrbanConfig.cpp - * @brief Tests for the CitySLAM configuration class - * @author Frank Dellaert - */ - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST( UrbanConfig, update_with_large_delta) -{ -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/cpp/testUrbanGraph.cpp b/cpp/testUrbanGraph.cpp deleted file mode 100644 index 4e7bab62c..000000000 --- a/cpp/testUrbanGraph.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/** - * @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 "Ordering.h" - -using namespace std; -using namespace gtsam; -typedef NonlinearOptimizer Optimizer; - -/* ************************************************************************* */ - -static Point2 landmark( 2, 5); -static Point2 landmark2( 2, 10); -static Point2 landmark3( -2, 5); -static 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*/ -static Pose3 robotPose(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.*/ -static Pose3 robotPose2(Matrix_(3,3, - 0., 1., 0., - 1., 0., 0., - 0., 0.,-1. - ), - Point3(0,1,0)); - -static boost::shared_ptr sensorMatrix(new Matrix(4, 4)); - -/* ************************************************************************* */ -TEST( UrbanGraph, addMeasurement) -{ - // Check adding a measurement - UrbanGraph g; - double sigma = 0.2;// 20 cm - g.addMeasurement(sensorMatrix, 4, 2, sigma, 1, 1); // ground truth would be (5,2) - LONGS_EQUAL(1,g.size()); - - // Create a config - UrbanConfig config; - config.addRobotPose(1,robotPose); - config.addLandmark(1,landmark); - - // Check error - double expected = 0.5/sigma/sigma; - DOUBLES_EQUAL(expected, g.error(config), 1e-9); -} - -/* ************************************************************************* */ -TEST( UrbanGraph, addOdometry) -{ - // Check adding a measurement - UrbanGraph g; - double sigmadx = 0.01; // 1 cm - double sigmayaw = M_PI/180.0;// 1 degree - g.addOdometry(2, 0, sigmadx,sigmayaw, 1); // 1m forward, 0 yaw - LONGS_EQUAL(1,g.size()); - - // Create a config - UrbanConfig config; - config.addRobotPose(1,robotPose); - config.addLandmark(1,landmark); - - // Check error - double expected = 0.5/0.01/0.01; - // TODO DOUBLES_EQUAL(expected, g.error(config), 1e-9); -} - -/* ************************************************************************* * - * 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(sensorMatrix, 5, 2, sigma, 1, 1); // z11 - g.addMeasurement(sensorMatrix, 10, 2, sigma, 1, 2); // z12 - g.addMeasurement(sensorMatrix, 5, -2, sigma, 1, 3); // z13 - g.addMeasurement(sensorMatrix, 10, -2, sigma, 1, 4); // z14 - g.addOdometry(1, 0, sigmadx,sigmayaw, 1); // 1m forward, 0 yaw - g.addMeasurement(sensorMatrix, 4, 2, sigma, 2, 1); // z21 - g.addMeasurement(sensorMatrix, 9, 2, sigma, 2, 2); // z22 - g.addMeasurement(sensorMatrix, 4, -2, sigma, 2, 3); // z23 - g.addMeasurement(sensorMatrix, 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, robotPose); - initialEstimate->addRobotPose(2, robotPose2); - initialEstimate->addLandmark(1, landmark); - initialEstimate->addLandmark(2, landmark2); - initialEstimate->addLandmark(3, landmark3); - initialEstimate->addLandmark(4, landmark4); - - // Check error vector - Vector expected = zero(16); - // TODO CHECK(assert_equal(expected, graph.error_vector(*initialEstimate))); - - // Check error = zero with this config - //DOUBLES_EQUAL(55, graph.error(*initialEstimate), 1e-9); - - // 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); - // TODO 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(); - // TODO DOUBLES_EQUAL(0, optimizer.error(), 1e-9); - - // check if correct - // TODO CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config()))); -} - -/* ************************************************************************* */ -int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0;} -/* ************************************************************************* */ diff --git a/cpp/testUrbanMeasurement.cpp b/cpp/testUrbanMeasurement.cpp deleted file mode 100644 index 2b4e7e42e..000000000 --- a/cpp/testUrbanMeasurement.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/********************************************************** - Written by Frank Dellaert, Dec 2009 - **********************************************************/ - -#include - -#include "UrbanConfig.h" -#include "UrbanMeasurement.h" -#include "Vector.h" - -using namespace std; -using namespace gtsam; - - -/* - * This unit test cross checks UrbanMeasurement with the - * results from Justin's thesis codebase. - */ - -static boost::shared_ptr sensorMatrix(new Matrix(4, 4)); - -static bool _data_inited = false; - -static void _init() { - // Not MT-safe. Don't think that's an issue for CppUnitLite - if (_data_inited) { - return; - } - (*sensorMatrix)(0, 0) = 0.0251; - (*sensorMatrix)(0, 1) = 0.9996; - (*sensorMatrix)(0, 2) = 0.0091; - (*sensorMatrix)(0, 3) = 1.5574; - (*sensorMatrix)(1, 0) = -0.9996; - (*sensorMatrix)(1, 1) = 0.0253; - (*sensorMatrix)(1, 2) = -0.016; - (*sensorMatrix)(1, 3) = -0.86; - (*sensorMatrix)(2, 0) = -0.0163; - (*sensorMatrix)(2, 1) = -0.0087; - (*sensorMatrix)(2, 2) = 0.9998; - (*sensorMatrix)(2, 3) = -0.6478; - (*sensorMatrix)(3, 0) = 0; - (*sensorMatrix)(3, 1) = 0; - (*sensorMatrix)(3, 2) = 0; - (*sensorMatrix)(3, 3) = 1; - _data_inited = true; - -} -/* 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*/ -static const Pose3 robotPose(Matrix_(4, 4, - -0.0848808908061426, 0.993833753552749900, 0.0713421661796702400, 377.7509309858, - -0.9963883079644135, -0.08449313834814816, -0.008440931458908668, 1922.5523404841, - -0.002360959078213238, -0.07180097402774338, 0.9974161849503438, -6369417.591562273, - 0.0, 0.0, 0.0, 1)); - -static const Point2 landmark(362.707949418, 1936.2843137418); - - - - -/* ************************************************************************* */ -TEST( UrbanMeasurement, transform_to ) -{ - _init(); - UrbanMeasurement::Transform tr = transform_to(sensorMatrix, robotPose, landmark, true); - - // Check the transformation from global to sensor coordinates - CHECK(assert_equal(Point2(-12.40679724375481, -16.14944758846734),*(tr.get<0>()))); - - Point2 &foo = *(tr.get<0>()); - - // Check the jacobian w.r.t. the pose - CHECK(assert_equal(Matrix_(2, 6, - 0.08471201853652961, 0.9964082882836194, 0.0, 0.03822695627705205, -0.5427133154140961, -16.15221468780303, - -0.998969460290058, 0.0851007754705408, 0.0, 0.5147498807397776, 0.0, 12.43765251715327), - *(tr.get<1>()))); - - // Check the jacobian w.r.t the landmark - CHECK(assert_equal(Matrix_(2, 2, - -0.08471201853652961, -0.9964082882836194, - 0.998969460290058, -0.0851007754705408), - *(tr.get<2>()))); -} - -/* ************************************************************************* */ -TEST( UrbanMeasurement, error_vector ) -{ - _init(); - - Point2 z(-12.42876033350471, -16.10191021850896); - double sigma = 0.2; - UrbanMeasurement factor(sensorMatrix, z,sigma,44,66); - - // Create a config - UrbanConfig config; - config.addRobotPose(44,robotPose); - config.addLandmark(66,landmark); - - // test error_vector - Vector expected = Vector_(2, - -.02196308974990, .04753736995838); - Vector actual = factor.error_vector(config); - CHECK(assert_equal(expected,actual)); - - // test error. Expected was generated via manual calculation from - // error vector. - double expected2 = .03427723560; - double actual2 = factor.error(config); - DOUBLES_EQUAL(expected2,actual2,1e-9); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - TestRegistry::runAllTests(tr); - return 0; -} -/* ************************************************************************* */ - diff --git a/cpp/testUrbanOdometry.cpp b/cpp/testUrbanOdometry.cpp deleted file mode 100644 index 12b7ef0b4..000000000 --- a/cpp/testUrbanOdometry.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/** - * @file testUrbanOdometry.cpp - * @brief Unit tests for UrbanOdometry Class - * @authors Frank Dellaert, Viorela Ila - **/ - -/*STL/C++*/ -#include - -#include -using namespace boost::assign; - -#include -#include "UrbanOdometry.h" -#include "Pose2Graph.h" - -using namespace std; -using namespace gtsam; - - -TEST( UrbanOdometry, constructor ) -{ - string key1 = "x1", key2 = "x2"; - Vector measured = zero(6); - Matrix measurement_covariance = eye(6); - UrbanOdometry factor(key1, key2, measured, measurement_covariance); - list expected; - expected += "x1","x2"; - CHECK(factor.keys()==expected); -} - - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */