diff --git a/cpp/UrbanFactor.cpp b/cpp/UrbanFactor.cpp index 7b96789ba..66a0f130b 100644 --- a/cpp/UrbanFactor.cpp +++ b/cpp/UrbanFactor.cpp @@ -11,7 +11,11 @@ namespace gtsam { UrbanFactor::UrbanFactor() { // TODO Auto-generated constructor stub + } + UrbanFactor::UrbanFactor(const Vector& z, const double sigma) : + NonlinearFactor (z,sigma) { + // TODO } UrbanFactor::~UrbanFactor() { diff --git a/cpp/UrbanFactor.h b/cpp/UrbanFactor.h index f2f59a3a4..6fcca91ae 100644 --- a/cpp/UrbanFactor.h +++ b/cpp/UrbanFactor.h @@ -13,20 +13,15 @@ namespace gtsam { - class UrbanFactor : public NonlinearFactor { + /** + * Base class for UrbanMeasurement and UrbanOdometry + */ + class UrbanFactor: public NonlinearFactor { public: + UrbanFactor(); + UrbanFactor(const Vector& z, const double sigma); 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; - } - }; } diff --git a/cpp/UrbanGraph.cpp b/cpp/UrbanGraph.cpp index c6c8d3188..bdd4a31d7 100644 --- a/cpp/UrbanGraph.cpp +++ b/cpp/UrbanGraph.cpp @@ -33,9 +33,10 @@ namespace gtsam { } /* ************************************************************************* */ - void UrbanGraph::addMeasurement(double x, double y, double sigma, int p1, - int p2) { - // TODO + void UrbanGraph::addMeasurement(double x, double y, double sigma, int i, int j) { + Point2 z(x,y); + sharedFactor factor(new UrbanMeasurement(z,sigma,i,j)); + push_back(factor); } ; diff --git a/cpp/UrbanGraph.h b/cpp/UrbanGraph.h index aa0d7fe6d..3848bcdef 100644 --- a/cpp/UrbanGraph.h +++ b/cpp/UrbanGraph.h @@ -8,7 +8,7 @@ #pragma once #include "NonlinearFactorGraph.h" -#include "UrbanFactor.h" +#include "UrbanMeasurement.h" namespace gtsam { @@ -35,13 +35,12 @@ namespace gtsam { /** * Add a landmark constraint */ - void addMeasurement(double x, double y, double sigma, int p1, int p2); + void addMeasurement(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 p); + void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int i); /** * Add an initial constraint on the first pose diff --git a/cpp/UrbanMeasurement.cpp b/cpp/UrbanMeasurement.cpp index ee0721c5e..e8a1123cb 100644 --- a/cpp/UrbanMeasurement.cpp +++ b/cpp/UrbanMeasurement.cpp @@ -5,71 +5,81 @@ * @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"); -} +namespace gtsam { -/* ************************************************************************* */ -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; -} + /* ************************************************************************* */ + Point2 transform_to(const Pose3& pose, const Point2& p) { + // create a 3D point at our height (Z is assumed up) + Point3 global3D(p.x(),p.y(),pose.translation().z()); + // transform into local 3D point + Point3 local3D = transform_to(pose,global3D); + // take x and y as the local measurement + Point2 local2D(local3D.x(),local3D.y()); + return local2D; + } + /* ************************************************************************* */ + Matrix Dtransform_to1(const Pose3& pose, const Point2& p) { + return zeros(2, 6); + } -// 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; -} + /* ************************************************************************* */ + Matrix Dtransform_to2(const Pose3& pose, const Point2& p) { + return zeros(2, 2); + } -/* ************************************************************************* */ -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; -} + /* ************************************************************************* */ + 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 i, + int j) : + UrbanFactor(z.vector(), sigma) { + 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 + Point2 hx = transform_to(pose,landmark); + return (z_ - hx.vector()); + } /* ************************************************************************* */ diff --git a/cpp/UrbanMeasurement.h b/cpp/UrbanMeasurement.h index 1612e37ad..0817229ea 100644 --- a/cpp/UrbanMeasurement.h +++ b/cpp/UrbanMeasurement.h @@ -7,94 +7,93 @@ #pragma once -#include "NonlinearFactor.h" -#include "GaussianFactor.h" -#include "Testable.h" -#include "Point2.h" +#include "UrbanFactor.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; + class UrbanConfig; /** - * Default constructor + * Non-linear factor for a constraint derived from a 2D measurement, */ - UrbanMeasurement(); + class UrbanMeasurement: public UrbanFactor { + private: - /** - * 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); + 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: - /** - * print - * @param s optional string naming the factor - */ - void print(const std::string& s="UrbanMeasurement") const; + typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor + //typedef boost::shared_ptr shared_ptrK; - /** - * equals - */ - bool equals(const UrbanMeasurement&, double tol=1e-9) const; + /** + * Default constructor + */ + UrbanMeasurement(); - /** - * predict the measurement (is that update ??) - */ - Vector predict(const UrbanConfig&) const; + /** + * 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); - /** - * calculate the error of the factor - */ - Vector error_vector(const UrbanConfig&) const; + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s = "UrbanMeasurement") const; - /** - * linerarization - */ + /** + * equals + */ + bool equals(const UrbanMeasurement&, double tol = 1e-9) const; - GaussianFactor::shared_ptr linearize(const UrbanConfig&) const { - //TODO implement linearize + /** + * 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_); + } }; - int getrobotPoseNumber() const { return robotPoseNumber_; } - int getLandmarkNumber() const { return landmarkNumber_; } + /** + * Transform 2D landmark into 6D pose, and its derivatives + */ + Point2 transform_to(const Pose3& pose, const Point2& p); + Matrix Dtransform_to1(const Pose3& pose, const Point2& p); + Matrix Dtransform_to2(const Pose3& pose); - - -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_); - } -}; - -} +} // namespace gtsam diff --git a/cpp/testUrbanGraph.cpp b/cpp/testUrbanGraph.cpp index 5a55a068a..168a46151 100644 --- a/cpp/testUrbanGraph.cpp +++ b/cpp/testUrbanGraph.cpp @@ -21,7 +21,7 @@ typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ -Point2 landmark1( 2, 5); +Point2 landmark( 2, 5); Point2 landmark2( 2, 10); Point2 landmark3( -2, 5); Point2 landmark4( -2,-10); @@ -29,7 +29,7 @@ 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, +Pose3 robotPose(Matrix_(3,3, 0., 1., 0., 1., 0., 0., 0., 0.,-1. @@ -45,8 +45,28 @@ Pose3 robotPose2(Matrix_(3,3, Point3(0,1,0)); /* ************************************************************************* */ -/* the measurements are relative to the robot pose so - * they are in robot coordinate frame different from the global (above)*/ +TEST( UrbanGraph, addMeasurement) +{ + // Check adding a measurement + UrbanGraph g; + double sigma = 0.2;// 20 cm + g.addMeasurement(4, 2, sigma, 1, 1); + 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); +} + +/* ************************************************************************* * + * 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 @@ -68,7 +88,7 @@ UrbanGraph testGraph() { return g; } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( UrbanGraph, optimizeLM) { // build a graph @@ -76,9 +96,9 @@ TEST( UrbanGraph, optimizeLM) // Create an initial configuration corresponding to the ground truth boost::shared_ptr initialEstimate(new UrbanConfig); - initialEstimate->addRobotPose(1, robotPose1); + initialEstimate->addRobotPose(1, robotPose); initialEstimate->addRobotPose(2, robotPose2); - initialEstimate->addLandmark(1, landmark1); + initialEstimate->addLandmark(1, landmark); initialEstimate->addLandmark(2, landmark2); initialEstimate->addLandmark(3, landmark3); initialEstimate->addLandmark(4, landmark4); diff --git a/cpp/testUrbanMeasurement.cpp b/cpp/testUrbanMeasurement.cpp index a9f1e2f25..28f1066fb 100644 --- a/cpp/testUrbanMeasurement.cpp +++ b/cpp/testUrbanMeasurement.cpp @@ -6,13 +6,53 @@ #include "UrbanConfig.h" #include "UrbanMeasurement.h" +#include "Vector.h" using namespace std; using namespace gtsam; +Point2 landmark(2,5); + +/* 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 robotPose(Matrix_(3,3, + 0., 1., 0., + 1., 0., 0., + 0., 0.,-1. + ), + Point3(0,0,0)); + /* ************************************************************************* */ -TEST( UrbanMeasurement, linearize ) +TEST( UrbanMeasurement, transform_to ) { + Point2 expected(5,2); + Point2 actual = transform_to(robotPose, landmark); + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( UrbanMeasurement, error_vector ) +{ + // Create a measurement, no-noise measurement would yield 5,2 + Point2 z(4,2); + double sigma = 0.2; + UrbanMeasurement factor(z,sigma,44,66); + + // Create a config + UrbanConfig config; + config.addRobotPose(44,robotPose); + config.addLandmark(66,landmark); + + // test error_vector + Vector expected = Vector_(2, -1.0, 0.0); + Vector actual = factor.error_vector(config); + CHECK(assert_equal(expected,actual)); + + // test error + double expected2 = 12.5; // 0.5 * 5^2 + double actual2 = factor.error(config); + DOUBLES_EQUAL(expected2,actual2,1e-9); } /* ************************************************************************* */