diff --git a/cpp/UrbanGraph.cpp b/cpp/UrbanGraph.cpp index bdd4a31d7..d78715a0e 100644 --- a/cpp/UrbanGraph.cpp +++ b/cpp/UrbanGraph.cpp @@ -8,6 +8,8 @@ #include "FactorGraph-inl.h" #include "NonlinearOptimizer-inl.h" #include "UrbanGraph.h" +#include "UrbanMeasurement.h" +#include "UrbanOdometry.h" namespace gtsam { @@ -43,7 +45,12 @@ namespace gtsam { /* ************************************************************************* */ void UrbanGraph::addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int p) { - // TODO + 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); } /* ************************************************************************* */ diff --git a/cpp/UrbanGraph.h b/cpp/UrbanGraph.h index 3848bcdef..d19a556f8 100644 --- a/cpp/UrbanGraph.h +++ b/cpp/UrbanGraph.h @@ -8,7 +8,7 @@ #pragma once #include "NonlinearFactorGraph.h" -#include "UrbanMeasurement.h" +#include "UrbanFactor.h" namespace gtsam { diff --git a/cpp/UrbanOdometry.cpp b/cpp/UrbanOdometry.cpp new file mode 100644 index 000000000..0db972253 --- /dev/null +++ b/cpp/UrbanOdometry.cpp @@ -0,0 +1,86 @@ +/** + * @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 new file mode 100644 index 000000000..8a329c496 --- /dev/null +++ b/cpp/UrbanOdometry.h @@ -0,0 +1,79 @@ +/** + * @file UrbanOdometry.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 "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_, "MeasurementCovariance"); + } + + 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; + 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/testUrbanGraph.cpp b/cpp/testUrbanGraph.cpp index 168a46151..517db24e4 100644 --- a/cpp/testUrbanGraph.cpp +++ b/cpp/testUrbanGraph.cpp @@ -50,7 +50,7 @@ TEST( UrbanGraph, addMeasurement) // Check adding a measurement UrbanGraph g; double sigma = 0.2;// 20 cm - g.addMeasurement(4, 2, sigma, 1, 1); + g.addMeasurement(4, 2, sigma, 1, 1); // ground truth would be (5,2) LONGS_EQUAL(1,g.size()); // Create a config @@ -63,10 +63,30 @@ TEST( UrbanGraph, addMeasurement) 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 @@ -88,7 +108,7 @@ UrbanGraph testGraph() { return g; } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( UrbanGraph, optimizeLM) { // build a graph @@ -103,23 +123,30 @@ TEST( UrbanGraph, optimizeLM) 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); - //DOUBLES_EQUAL(0, optimizer.error(), 1e-9); + 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(); - // DOUBLES_EQUAL(0, optimizer.error(), 1e-9); + // TODO DOUBLES_EQUAL(0, optimizer.error(), 1e-9); // check if correct - // CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config()))); + // TODO CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config()))); } /* ************************************************************************* */